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ABSTRACT 


A six degree of freedom robot manipulator arm, a PUMA 560, is calibrated using full 
pose and partial pose methods in order to improve the accuracy of the manipulator arm. 
The theory applicable to modeling of mechanisms is introduced. A thirty parameter 
kinematic model is developed for use in the full pose calibration method and a twenty-six 
parameter kinematic model is developed for the partial pose calibration. A simulation study 
is performed to determine the applicability and feasibility of each model. Experimental 
pose measurements are performed using each method to obtain data with which to perform 
an actual calibration of the manipulator and compare with the predicted results. The effects 
of noise in each measurement system employed and in the manipulator's joint position 
encoders are discussed. The measurement systems employed are examined in detail and a 
comparison between the two is performed. 


The measured kinematic parameters of each model are presented as results. 
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I. INTRODUCTION 


In the calibration of a manipulator the desired result is to significantly improve the 
manipulator's accuracy. Accuracy is defined as the ability of a robot to move to a 
commanded pose defined in the manipulator's working volume [Ref. 1]. Stated simply. 
how accurately can the origin of a coordinate frame and the orientation of the coordinate 
axes attached to the end effector of the manipulator arm, to be termed achieving a certain 
pose, be positioned in relation to a commanded point and orientation in the manipulator's 
working volume? Present experimentation shows that manipulators have an accuracy oí 
about 10.0 mm [Ref. 2]. 

Another indication of a manipulator's ability to achieve a pose is it's repeatability. 
Repeatability is the ability of a manipulator to return to a previously achieved pose [Ref. 3]. 
To illustrate repeatability, consider the following. A manipulator arm's end effector, 
termed the tool, is moved to a certain position and orientation in the working volume. After 
the position of each joint angle of the manipulator is recorded, the tools position and 
Orientation is altered. The manipulator is commanded to assume the previously recorded 
joint angles. Examination will reveal that the tool's position and orientation will differ from 
the originally learned pose. This is the error in repeatability of a manipulator arm. Present 
experimentation shows that manipulators have an repeatability of around 0.3 mm [Ref. 4]. 

To summarize, the fundamental difference between accuracy and repeatability is that 
repeatability is the ability of the manipulator to return to a previously achieved pose and 
accuracy is the ability of the manipulator to move to a pose that is specified in the working 
volume and that may have never been previously reached. It is apparent that a more 
accurate manipulator is desired over a more repeatable manipulator. The accurate 


manipulator's working volume is unconstrained while the repeatable manipulator does not 


have a working volume, only discrete positions which must be taught to the manipulator. 
Correspondingly, the objective of the calibration process is to improve the accuracy of the 
manipulator. 

In order to understand how a calibration is performed on a manipulator one must 
understand how a commanded pose would be achieved by any manipulator. When a 
specific position and orientation is commanded to the manipulator, it must be specified 
relative to a world coordinate frame defined by the user. To the manipulator this defined 
world coordinate frame is, as described, the center of the universe. By means of several 
mathematical operations the coordinate frame originally attached to the world coordinate 
frame is moved from the world coordinate frame to the manipulator’ base and through each 
arm of the manipulator until it reaches the tool. This implies a knowledge of a mathematical 
model of the manipulator which includes a world coordinate frame. 

As itis prohibitively expensive to manufacture and assemble each manipulator to the 
exact specifications required for desired accuracy, every manipulator built has a unique 
geometry. Since each manipulator has this unique geometry, each will have a unique 
mathematical model. The calibration process will identify the kinematic parameters 
embedded within the manipulator in order that the individual] mathematical model will 
accurately represent the physical manipulator. 

In the calibration process, several sequential steps enable the precise kinematic 
parameters of the manipulator to be identified, leading to improved accuracy. These steps 
are described as follows [Ref. 5]: 

1.  Akinematic model of the manipulator and calibration process is developed. The 
resulting model is used to define a pose error quantity based on a nominal kinematic 
parameter set, an unknown parameter set representing the true geometry of the manipulator 


and a set of joint angles. The nominal kinematics would be supplied by the manufacturer of 


the manipulator, while the unknown, actual parameter set will be identified in the 
calibration process. 

2. Experimental measurements of the robot pose, which could be either full pose 

Or partial pose, are taken in order to obtain data which correspond to the actual kinematic 
parameters of the manipulator. 
5. The actual kinematic parameters are identified by systematically changing the 
nominal parameter set so as to reduce the error quantity defined in the modeling phase. 
This is performed as a multidimensional optimization search in which the identified 
kinematic parameters are changed in order to reduce an error function to a minimum 
amount. 

4. The final step involves incorporating the identified parameters into the actual 
controlling software of the manipulator. 

Once the calibration process is complete the next step is to implement the identified 
kinematic parameters into the operation of the manipulator arm. In an operational setting 
the required position and orientation of the manipulator arm's end effector would be input 
to a controlling software program. This control program would perform an inverse 
kinematic solution through the mathematical model of the manipulator encoded within the 
control scheme, calculating the required joint angles to position and orient the end effector 
as dictated by the operator. 

This work addresses the issue of developing the kinematic model which represents 
the PUMA manipulator arm and gathering the experimental data used in the calibration 
process. Two methods will be described in this thesis: a full pose calibration process and a 
partial pose calibration process. 


The full pose calibration process will provide the benchmark calibration since the full 


pose of the end effector will be measured in each observation. 


Once the benchmark kinematic parameters are identified the second method of 
calibration, the partial pose calibration will be performed. The resulting kinematic model 


produced by this calibration will be contrasted with the values identified in the benchmark. 


Il. THEORY 


A. MODELING 

The theory presented and several of the diagrams in this chapter follows closely that 
presented by Paul [Ref. 6] in Chapters I through IIl. Also, several diagrams and 
explanations in this chapter are based on readings from Mooring, Roth, and Driels [Ref. 
E. 

1. Homogeneous Transformations 

The most basic premise on which the calibration process is associated is the 

movement of a set of coordinate axes from one position and orientation in space to another 
position and orientation, as illustrated in Figure 1. In Figure 1, the coordinate axes at point 


A are transformed, via translations and rotations, to point B. 


] 
à Zi 


x2 


y1 


y2 
Ζ2 


Figure 1. A Homogeneous Transformation 
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In addition to the translation of the origin of the coordinate axes from one 
position in space to another there is a change in the orientation of the coordinate axes. The 
process of the movement of the coordinate axes is known as a homogeneous 
transformation. This homogeneous transformation is accomplished by a series of 
translational transformations and rotational transformations. 

The first type of transformation is the translational transformation. Given a 
point in space, A, illustrated in Figure 2, the point's position is fixed and known relative to 
a fixed, three axis coordinate frame, defined as the world coordinate frame, by means of 


the following vector, u: 


T 
i-i 
οὔ. (1) 
y 
u 


Z 


Figure 2. Position of Point A with respect to a World Coordinate Frame 


The matrix element w is any non-zero scale factor. In the applications reported 


in this thesis w will be set equal to unity, producing the following vector: 


: (2) 


The transformation from point A to point B, illustrated in Figure 3, requires the 


transformation matrix |H |: 


Du 
H = Trans(x,y.z)=: 001c 
0001. (3) 
The translation from point A to point B is represented by the vector v: 
v = ait bjt+ck (4) 


The transformed vector is the product of the transformation matrix, H, and the 


original position vector, u. 


y= Hiu 
100a 
ER 10b [3| 
ano 0 hou 7. 
ι000 111. (5) 


The translation is to be interpreted as the summation of the vectors u and v. 


This is illustrated in Figure 3 and described in equation form by equation (6). 


“| 
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Figure 3. Translation of a Point in Space from Point A to Point B 


ce ees Y 2 EM 
usves (3 ji + E +2 )k + ai + bj + ck, (6) 
The second type of transformation is the rotational transformation. Given the 


fixed coordinate frame of the previously discussed translational transformation the 


transformation corresponding to a rotation, 6, about the x-axis is: 


p 


] 0 0 0 
Γιο. 0 cos(6) -sin(0) 0 
| O οἰπ(ίθ) cos0) 0 

0 0 0 τών (7) 


The transformation corresponding to a rotation, 0, about the y-axis is: 


T 


| cos(6) 0 sin(@) 0 
Rot(y,8) - 0 1 0 0 
| -sinf®) 0  cos(0) O | 
| 
0 0 0 BTE (8) 
The transformation corresponding to a rotation, 8, about the z-axis is: 
| cos(8) -sin8) 0 0 | 
«ΙΠ(0) cos(0 0 0 
Rot(z,0) - » = 
0 0 1 0 | 
ERU 0 0 τ. (9) 


As an illustration of a rotational transformation consider the following. Given 
another point in space, A. illustrated in Figure 4. if it was desired to rotate the point around 
the z-axis by 90 degrees the Rot(z) matrix above would be used. The value for 0, 90 
degrees, would be entered into the matrix, producing: 


0- 


00 
Rot(z,90°) =; 4 900. 
0 001. 


eco 


(10) 


This matrix would multiply the vector representing the original position of point 


IS (11) 





Figure 4. Rotation of Point A 90 Degrees about the Z-axis, Rot(z,90) 


Translational and rotational transformations are performed right to left as they 


are read, for example: 


1 0:0:2:5080. 7 02 09 EUN 

Trans(a,b.c)Rot(y,90)Rot(z,90) = οι 5. οιοουο ο... 

| (00016c/-2000/,0010. 
.0001/00017000 1J (13) 


The transformation illustrated in Figure 5 is a a rotation of 90 degrees about the 
Z-axis, a rotation of 90 degrees about the y-axis, a translation of ‘a’ mm in the x direction, 


of 'b' mm in the y direction, and of 'c' mm in the z direction. 


10 





Figure 5. A Multiple Translation and Rotation Transformation of Point A 


It is clear that an infinite number of translations or rotations are permitted. Each 
transformation is simply represented by another matrix multiplication operation. 
2. Application of Basic Homogeneous Transformations to Simple 
Kinematic Mechanisms 
Armed with the knowledge of homogeneous translations and rotations. a 
genera, two link kinematic mechanism will be analyzed. When homogeneous 
transformations are applied to the linkage shown in Figure 6, equations (3) and (9) will be 
utilized to move coordinate frame 1, located at point 1, to coordinate frame 2, located at 


point 2, by specifying the multiple transformation: 


A = Trans(dj,0,0) Rot(z,6;) Trans(d2,0,0) (14) 


11 


The product of the three matrix multiplications, a frame to frame homogeneous 
transformation, is known as an A matrix. Referring to Figure 6, the coordinate frame at 
point 1 has been assigned such that the x-axis lies along the centerline of link 1. As 
illustrated in equation (14), the homogeneous transformation moves the coordinate frame 
along the x-axis a distance dj, rotates the frame about the z-axis 61 degrees, and then 


translates the coordinate frame a distance d? along the newly rotated x-axis. 


y 
2 
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| | 2 
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Figure 6. A Two-Link Kinematic Mechanism 


For more general link structures, which most likely will be three dimensional, 
the coordinate frame transformations are more complex than the example shown in Figure 
6. Due to this complexity, a standardization of the allocation of coordinate frames and the 
transformation process of the coordinate frames is required. These standardizations will be 


discussed in the next section. 


3. Links, Joints, and Assignment of Coordinate Frames 

Any manipulator can be described as a series of links connected together by 
joints. A coordinate frame is placed on each link in the manipulator and, as discussed 
earlier, homogeneous transformations are used to describe the relative position and 
orientation difference between two successive links. Each homogeneous transformation 
matrix operation rotates or translates the coordinate frame to various positions in the 
manipulator. 

The base of a manipulator is defined to be link O and is not considered to be one 
of the defined number of links composing the manipulator. In between the base of the 
manipulator, link O, and the first joint of the manipulator is link 1. Thus, link O is attached 
to link 1 by joint 1. Link 2 is attached to link 1 by joint 2, and so on. 

A link is characterized by two dimensions: the common normal distance, ap. 
and the angle, Gn, between the axes in a plane perpendicular to an. The common normal 
distance is known as the length of the link and the angle is known as the twist of the link. 


These two link dimensions are illustrated in Figure 7. 


Joint n Joint n+! 





Common normal distance 


Figure 7. The Length, a, and Twist, a, of a Link 
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To aid in the visualization of these two parameters perform the following. 
Visualize a small diameter bar of reasonable length. At each end of the bar is a joint which 
pivots. Now the bar looks like a goal post with short uprights and a long crossbar. Take 


the uprights and bend them in. The bar will appear as Figure 8. 


z 


ex 


n 


Figure 8. Illustration of Link Dimensions a and a 


The next step is to grasp the uprights and twist them in opposite directions. The 
length of the line which connects the left upright to the right upright, while maintaining 
right angles with each, is an. To visualize Qn, draw a line which is parallel to the left 
upright through the point where the crossbar is attached to the right upright. The angle 
between the line which has been drawn and the right upright is the angle of twist, Gn. 

As stated earlier, any manipulator is composed of a series of links which are 
attached bv joints. The relationship between links is described using the distance and angle 
between links. The distance between the links is defined as dp and the angle between two 
successive links is defined as On. Refer to Figure 9 in the following discussion. 

Two links will be connected at a joint. The joint will have a joint axis, around 
which the two links will rotate. A normal from each link intersects the joint axis. The 
relative position of two successive links is defined as dp, the distance separating the points 
of intersection on the joint axis of the normals of the two links under consideration. Define 


the normal of the link to the left of the joint as N1. Define the normal of the link to the right 
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of the joint as N2. Extend Ny through the intersection of the joint axis. Draw a line 
parallel and coplanar to N2 through the point of intersection of Ny with the joint axis. The 


angle between the extension of Nj and the line parallel to N2 is defined as 05. 


Joint n Joint n+! 


Bn > Orel 







Joint n-| 


Link n-| 


Figure 9. Link Parameters 6, d, a, and a 


In order to describe the relationships between links, a coordinate frame will be 
assigned to each link, based on the type of joint to which the link is attached. There are 
two types of joints, revolute and prismatic. 

Consider a revolute joint, illustrated in Figure 10. Revolute joints are 
characterized by Op, the joint variable. The origin of the coordinate frame of link n is set to 
be at the intersection of the common normal between the axes of joints n and n+1, which 


will be the same line as the one measured for the length of link n, extended if necessary, 


| 


and the axis of joint n+1. In the case of intersecting joint axes, the origin is at the point of 
intersection of the joint axes. If the axes are parallel the coordinate origin is not uniquely 
defined if the above specifications are used, so the origin is chosen to make the joint 
distance zero for the next link whose coordinate origin is defined. The z-axis for link n will 
be aligned with the axis of jointn+1. The x-axis will be aligned with any common normal 
which exists and is directed along the normal from joint n to joint n+1. In the case of 
intersecting joints, the direction of the x-axis is parallel or antiparallel to the vector cross 


product zp-1 X Zn. 





World 


Figure 10. Revolute Joint 


Now consider prismatic a joint, which is illustrated in Figure 11. A prismatic 


joint is characterized by the distance dp being the joint variable. The direction of the joint 
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axis is the direction in which the joint moves. The direction of the axis is defined but, 
unlike a revolute joint, the position in space is not defined. In the case of a prismatic joint 
the link length, ag, has no meaning and is set to be zero. The origin of the coordinate 
frame for the prismatic joint is coincident with the next defined link origin. The z-axis of 
the prismatic link is aligned with the axis of jointn+1. The xp-axis is parallel or antiparallel 


to the vector cross product of the direction of movement of the prismatic joint and Zp. 





x World 


Figure 11. Prismatic Joint 


4. The Denavit-Hartenburg Transformation 
Now that a coordinate frame has been assigned to each joint of the manipulator, 
the next step is to develop the relationship between coordinate frames. The relationship 


between coordinate frame n and n-1 will now be examined in detail. In the calibration 
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processes reported in this thesis two types of relationships will be used. They are known 
as the modified Denavit-Hartenburg (MDH) transformation, a frame to axis transformation, 
and the Euler transformation, a frame to frame transformation. 

The modified Denavit-Hartenburg transformation is a derivative of the standard 
Denavit-Hartenburg transformation. The standard Denavit-Hartenburg (DH) 
transformation incorporates the translations and rotations introduced earlier in the thesis, 


and result in an A matrix of the following form: 


An = Rot(z,6,) x Trans(z,d,) x Trans(x,a,) x Rot(x,ay) (15) 


This is to be interpreted as: 

1. A rotation of angle Op about the z-axis. 

2. A translation of length dg along the z-axis. 

2. A translation of length ap along the newly rotated x-axis. 

4. Arotation of angle a about the newly rotated x-axis. 

The standard Denavit-Hartenburg transformation is not implemented in the 
modeling of manipulator arms for use in the calibration procedures described in this thesis. 
As described earlier, the location of coordinate frames are functions of manipulator 
geometry. Any variation in the manipulator geometry will cause a cascading change in the 
positions of the coordinate frames. This results in three effects which are undesirable for 
manipulator calibration: 

]. Selection of the base frame is not arbitrary 

2. The zero position of the manipulator is not arbitrary. 

3. If two joints having nominally parallel axes are found to have non-parallel 


axes, then the transformation parameters will dramatically change. 
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The third result is the most important. Many manipulators are designed to have 
consecutive parallel axes. In this case, there is no common normal between the two axes. 
In conforming to the rules introduced earlier the coordinate frame is chosen such that the 
joint distance will be zero for the next link whose coordinate link whose origin is defined. 
In almost every calibration procedure a misalignment of the two nominally parallel joint 
axes will be identified. A misaligned pair of joint axes produces a common normal 
between them, and the common normal fixes the position of the coordinate frame. The 
new, fixed position of the coordinate frame could be quite different from the user defined 
position of the coordinate frame based on the joint distance being zero. This could cause a 
cascading change in position of coordinate frames downstream of the coordinate frame in 
question. Such significant changes in several positions of coordinate frames causes 
difficulty in the kinematic parameter identification program. 

The problem with the parallel axes is eliminated by using the MDH 
transformation, which introduces a fifth matrix multiplication in the A matrix, as described 
in the following section. 

5. The Modified Denavit-Hartenburg Transformation 
The MDH transformation is, as stated earlier, a frame to axis transformation and 


is represented by the following A matrix, 


A, = Rot(z,0,) x Trans(z,d,) x Trans(x,a,) x Rot(x,a,) x Rot(y,B,) (16) 


This is to be interpreted as: 
1. A rotation of angle 8p about the z-axis. 
A translation of length dp along the z-axis. 


A translation of length ap along the newly rotated x-axis. 


ee 


A rotation of angle a about the newly rotated x-axis. 
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5. A rotation of angle Bn about the newly rotated y-axis. 
In reduced matrix form, for a rotational joint, the operation is as follows: 


cos(8)  -sin()cos(a) sin(8)sin(o) ^ acos(8) | 
sin(0) cos(8)cos(a) -cos(0)sin(a) ^ asin(O)  . 
| 
| 


E 
0 sin(a) cos(a) d 
| 0 0 0 ] (17) 
In reduced matrix form, for a prismatic joint, the operation is as follows: 
| cos(0) -sin(@)cos(a) sin(@)sin(a) 0 | 
«ἰπ(θ) | cos(0)cos(a) -cos(6)sin(a) 0 | 
| 0 sin(a) cos(a) d 
" 0 0 0 ] i (18) 


6. The Euler Transformation 
The Euler transformation is a frame to frame transform which requires six 


parameters: 


An = Rot(z,g,) x Rot(y,6,) x Rot(x,@,) x Trans(px,, py,» Pz,) (19) 


This will be interpreted as a rotation 9n about the z-axis, followed by a rotation 
On about the newly rotated y-axis, followed by a rotation Pn about the newly rotated x- 
axis. At the conclusion of these rotations the axes of frame n, the coordinate frame to 
which the transform is occurring, will be perfectly aligned with the transforming coordinate 


frame. The translations px, py, and pz will move the aligned coordinate axes from the 


origin of coordinate frame n-1 to coordinate frame n. 
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7. Application to the PUMA 

It has been established that an A matrix is a homogeneous transformation from 
one coordinate frame to the next. This A matrix can either be, in applications described in 
this thesis, a modified Denavit-Hartenburg transformation matrix or an Euler 
transformation matrix. Matrix Aj will describe the position and orientation of the first 
coordinate frame with respect to the world coordinate frame. The coordinate frame attached 
to link O is translated and rotated such that it is aligned in the proper position and 
orientation, in accordance with the rules described earlier, on link 1 of the manipulator. 
Matrix A? will describe the position and orientation of the second coordinate frame with 
respect to the first coordinate frame. The coordinate frame attached to link 1 is translated 
and rotated such that it is aligned in the proper position and orientation on link 2 of the 
manipulator. Thus, the position and orientation of the second coordinate frame with 


respect to the base coordinate frame is defined to be: 


Since the PUMA 560 manipulator is a six link manipulator, the T6 matrix, 
which the indicate the position and orientation of the tool frame with respect to the world 


coordinate frame, will be represented by: 


Ts = Aix Ax As X Aqax As X Áo (21) 


Figure 12 illustrates that any coordinate frame may be reached from any other 
coordinate frame by executing the matrix multiplications between the initial coordinate 


frame and the final coordinate frame, moving clockwise or counter-clockwise. 
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Figure 12. Homogeneous Transformation Loop 


Moving clockwise would generate a forward kinematic solution, equivalent to 
moving through the manipulator from, for example, coordinate frame 3 to the tool frame. 
Moving counter-clockwise will generate an inverse kinematic solution. equivalent to 
moving backwards through the manipulator from coordinate frame 4 to coordinate frame 2. 

With the acquired knowledge of links, joints, how to assign reference frames 
and transform from one reference frame to the next, the PUMA 560's coordinate frames 
and the nominal kinematic parameters will be introduced. 


Figure 13 illustrates individual links and joints of the PUMA 560. 
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Figure 13. Links and Joints of the PUMA 560 Robot Manipulator Arm 


If all of the rules governing the placement of coordinate frames are followed, 
the position and orientation of the coordinate frame corresponding to each joint may be 


displayed. Figure 14 illustrates the allocation of each coordinate frame. 
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Figure 14. PUMA Frame Allocation 


The internal kinematics of the PUMA will remain constant regardless of the 


world coordinate frame used or type of measurement system employed. Refer to Figure 14 
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to follow the discussion of the transformation from the base coordinate frame of the 
manipulator to frame 5. Note that the position of the manipulator depicted in Figure 14 
does not show the manipulator in its zero position. The transforms which will be described 
in the next paragraph will move from joint n-1's zero position to joint n's zero position. 
All of the transformations within the PUMA are MDH transformations. In both of the 
calibration procedures reported in this thesis the transformation from the world coordinate 
frame to the base frame and the transformation from frame 5 to frame 6, the tool frame, is 
an Euler transformation. The details of those transformations will be discussed during the 
simulation phase of each calibration procedure. 

1. Base frame, frame 0, to frame 1: No rotation about Zo, no translation along 
the ZQ-axis, no translation along the XQ-axis, rotate -90 about XQ. This fixes coordinate 
frame 1. 

2. Frame 1 to frame 2: No rotation about Z , no translation along the Z}-axis, 
translate along the X1-axis 431.85 mm, no rotation about X1. This fixes coordinate frame 
D. 

3. Frame 2 to frame 3: No rotation about Z2, translate along the Z2-axis 
149.09 mm, translate along the X2-axis -20.33 mm, rotate 90° about X2. This fixes 
coordinate frame 3. 

4. Frame 3 to frame 4: No rotation about Z3, translate along the Z3-axis 433.0 
mm, no translation along the X3-axis, rotate -90 about X3. This fixes coordinate frame 4. 

5. Frame 4 to frame 5: No rotation about Z4, no translation along the Z4-axis, 
no translation along the X4-axis, rotate 90 about X4. This fixes coordinate frame 5. 

Since the coordinate frames have been assigned to each joint and the 
relationship between each frame has been determined, a table of nominal kinematic 


parameters may be constructed. Note that the transform from the base frame to frame 1 and 
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the transform from frame 5 to the tool frame are Euler transformations. All other 
transformations are Modified Denavit-Hartenburg transformations. The bold entries are 


defined to be zero. 


TABLE 1. NOMINAL KINEMATIC PARAMETERS FOR THE PUMA 560 
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At this point the concept of the forward kinematic solution and the inverse 
kinematic solution will be introduced. A forward kinematic solution is necessary to 
determine the position and orientation of the tool frame with respect to the world coordinate 
frame if all six joint angles are known. Given six joint angles, the pose can be determined 
using a forward kinematic solution. An inverse kinematic solution is necessary to 
determine the six required joint angles which would place the the tool frame in a specified 
position and orientation. Given the tool pose, the six joint angles can be determined using 


an inverse kinematic solution. 
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B. IDENTIFICATION METHODOLOGY 
1. Introduction 


Consider a generic function in the x-y plane, illustrated in Figure 15. 





Figure 15. Generic Function in the x-y Plane 


If a relative maxima of the function was to be determined within a range of x 
then the derivative of the function could be calculated at some x within that range. If the 
derivative was positive then the maxima would be to the right of the x value and if the 
derivative was negative then the maxima would be to the left. The x value would be 
incremented and the derivative calculated until the the derivative equaled zero. The function 
would be optimized at this point within the specified range of x. 

This concept can be extended to surfaces. The problem of climbing a hill in the 
most efficient manner is one example. Consider a generic two variable function illustrated 


in Figure 16. 
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Figure 16. Example of Two-Variable Function G(x,y) 


It is desired to find maximum G. To determine the direction of maximum 
increase of the function G at a specific point, calculate the gradient of the function and 


evaluate the gradient at the point in question: 


VG(xi,y1) = 








uo i ] 
OX /xy.y; Oy /x,y, (22) 


Calculating the gradient provides the direction of maximum increase based on 
(x1+Ax), (x1-Ax), (y1+Ay), and (y1-Ay). 

A question arises: How is the gradient calculated if G(x,y) is not specifically 
known? The only way to determine the gradient is through a method of orderly estimation 


and elimination. For a two-dimensional function this provides a challenge. If the problem 
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is extended to n-dimensional space the problem is extremely difficult to solve. The 
difficulty in n-dimensional optimization is the development of an algorithm which will 
change the n variables of the function in an efficiently and orderly manner in order that the 
gradient can be determined. 
2. The IMSL routine ZXSSQ 

The optimization procedure used in this thesis is an IMSL routine called 
ZXSSQ. ZXSSQ is a routine which will vary the n variables of a function in order that the 
function will be minimized. As a basic example of the operation of ZXSSQ consider the 


following equation: 


y = (X,x Z}+ sin (X2 x Z) (23) 


This equation will be the model of some physical system. The parameters X] 
and X2 are to be determined based on the observation of y at incremental values of Z. The 


following observations are taken: 


y, = 1 + sin(1)+ (-0.1) = 1.74 
y2 = 2 +sin(2)+ (0.1) = 3.00 
y3 = 3 + sin (3) + (-0.1) = 3.04 
y4 = 4 + sin (4) + (0.1) = 3.34 


y5 = 5 + sin (5) + (-0.1) = 3.94 (24) 
which is: 
y= X, x Z + sin(X2 x Z) = 0.1 (25) 
with: 
TS 
X521 (26) 
The model predicts: 
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yı = 1 + sin (1) = 1.84 
yo = 2+sin (2) ae 

y3- 2 t sIn (3) 7M 

y4 = 4 + sin (4) = 3.24 

ys = 5 + sin (5) = 4.04 (27) 


ZXSSQ will not discriminate the addition or subtraction of 0.1. This is 
equivalent to the presence of noise in a measurement system. So, ZXSSQ will use the 


flowchart in Figure 17 to select the values of X1 and X2 which will best fit the above data. 


select initial 
ΧΙ and A2 


calculate 


yi predicted 


select new 
X1 and X» 


Ei = Yi measured i Yi calculated 





XA, 


print 


yes X1 and X5 


Figure 17. Flowchart of the Operation of ZXSSQ 


The initial values of Xj and X2 are defined by the user. For each observation, 
producing à Yireasured» 4nd à Yipredicted will be calculated based on the current X1 and 
X2. The difference between Yipredicted and Yimeasured Will be calculated for each 
observation and the sum of these differences will be calculated. If this sum is less than the 
user defined convergence criteria the current values of X1 and X2 are acceptable. If not 
ZXSSOQ selects new values for X1 and X2 and the process begins again. The process of 
the selection of the values of X1 and X2 performed by ZXSSQ is a mathematical 
consideration which is not important to the results reported in this thesis, but it must be 
understood that ZXSSQ makes the identification of the kinematic parameters of the PUMA 
within a reasonable period period of time possible. 

As an illustration of the implementation of ZXSSQ in this thesis, consider the 
following. Suppose a ball is suspended in space in an unknown position relative to a 


defined set of coordinate axes, as illustrated in Figure 18. 
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Figure 18. Determination of the Center of a Ball in Space 
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It is possible to use a touch probe to measure (*i- Yi: Zi), the coordinates of an ith 
position on the surface of the ball relative to the fixed coordinate frame. However, the exact 
position of the center of the ball is required in order to assist in the determination of the 
pose of the tool. ZXSSQ accepts the coordinates of at least three surface positions on the 
ball and calculates the coordinates of the center of the ball. The procedure is as follows. 

The end effector used in the experimental phase of the initial calibration has a 
pattern of precision tooling balls which have a known radius, Rip. The toüch probe also 
has a known radius, Rip. That means that the center of the ball will be (Rtb + Rip) from 
each measured position of the touch probe. If only one touch probe measurement is made 
the actual position of the center of the ball could be at any position on a sphere of (Rtb + 
Rip) radius surrounding the single measurement. As more measurements are taken the 
position of the center of the ball can be determined more accurately. It has been determined 
that a minimum of three touch probe measurements are required to accurately determine the 


coordinates of the center of the ball. Each measurement produces an error function: 


F, =|(Rib + Rip) - V (Xe -x1 2 + (Ye) nF + (Ze - rl 
Fz = | (Riv + Rip) - ^ (xc -xaP 4 (γε -γ) + (Ze -22 | 


F3 = (Rus + Ru) - ^ (xc x3F. * (yc -YaP. * (zc -z3F = (28) 


ZXSSQ will systematically vary (Xo. Ye.Zc) until each Fj is minimized. If there 
is no noise in the system, each Fj will be reduced to zero. If noise is present the best fit 
(Χο Υο»Ζς) will be calculated. Once the functions are minimized, the coordinates of the 


center of the ball are known. 
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3. Application to the Calibration Process 
The kinematic parameter identification of the PUMA 560 will be performed as a 
multi-dimensional minimization process performed by ZXSSQ. The process will proceed 
in the following manner. 
1. Begin with the nominal set of kinematic parameters. This set will most 
likely be the manufacturer's predicted parameters based on the design and construction of a 


generic manipulator. 


tO 


Select a set of six joint angles, 01 through 06, for the manipulator. 
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Perform a forward kinematic solution for the PUMA. using the nominal set 
of kinematic parameters. This will calculate the predicted pose of the end effector of the 
manipulator. 

4. Measure the actual pose of the end effector of the manipulator. In most 
cases measured pose will be different from the predicted pose. 

5. Modify the kinematic paramcters such that the predicted pose, determined 
by the forward kinematic solution using the modified kinematic parameters, matches the 
measured pose [Ref. 8]. 

This process will be applied to several sets of joint angles. The number of 


physical measurements, meaning the number of sets of joint angles required, must satisfy: 


Kp Nx Dy (29) 


where: Kp = number of kinematic parameters to be identified 
N = number of measurements (poses) taken 
Df = number of degrees of freedom present in each measurement 
Two calibration processes will be reported in this thesis. Each of the calibration 


procedures will have a simulation phase and an experimental phase. The details of each 


kinematic identification process will be described in each section of the appropriate 
calibration procedure. Refer to the preceeding paragraph for a general explanation of the 


kinematic parameter identification procedure. 
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IHI. FULL POSE CALIBRATION 


A. THEORY 
1. Introduction 

As stated earlier, the nominal coordinate transformations from the base frame to 
frame 5 will not change, regardless of the type of calibration process used or the type of 
measurement system used in the calibration. The transformation from the world coordinate 
frame to the base frame is an Euler transform, as well as the transform from frame 5 to 
frame 6, the tool frame. 

The transformation from the world coordinate frame to frame 1 of the 
manipulator needs to be carefully considered, since there are potential parameter 
dependencies if certain transforms are chosen. Consider Figure 19 which shows the world 
coordinate frame xw, yw,Zw, frame x0, y0,z0, frame xb, yb,Zb, and frame x1,y1,Z1. 

Frame x(.y0,z0 is defined bv a DH transform from the world frame to the first 
joint axis of the manipulator, frame xp. yb.zb is the PUMA manufacturer's base frame and 
frame x1,y 1,21 is the second DH frame of the manipulator. What are the minimum number 
of parameters that are required to move from the world frame to frame x1,y 1,21? There are 
two transformation paths which will accomplish this transform [Ref. 9]. 

Path 1: A DH transform from the world coordinate frame to frame x0.y0.20 
involving four parameters followed by another DH from frame x(,y0,z0 to frame xb,yb.Zb 


which will involve only two parameters 9 andd in the transform: 


T? - rot(zo, $ )trans(zo.d') (30) 
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Figure 19. Base Transformations 


Another DH transform from Xb,yb,Zb tO X],y],Z] which involves four 
parameters. Note that 49; and ? both rotate about zg. That means that a rotation 
A0, about the z(-axis is indiscernible. Any rotation about the z-axis can be accomplished 
using an infinite number of combinations of values of 48; and Φ᾽ which means that 
A0; and 9' can not be identified independently. This leads to a conclusion that eight 
independent kinematic parameters are required to move the coordinate frame from the world 
frame to the first frame of the PUMA using this path. 

Path2: A transform can be defined directly from the world coordinate frame 
Xw,Yw.Zw, to the base frame Xp,yb.zp. This is a frame to frame transform so an Euler 


transform is required: 


Ap 7 rot(z, Qo)rot(y,Op)rot(x,qo)trans(pxe. yb. Dzb) (31) 


The DH transform from xb,yb.zb to x1,y1,Z1] which will follow would 
normally involve four parameters, but there is another dependency involving A801 and 
Ad;, A0; can be resolved into $b» 9b» €» and Ad; can be resolved into (Pxb»Pyb»Pzb), This 
reduces the number of parameters required for the transform from the base frame to frame 1 
from four to two. Coupled with the six parameters required to transform from the world 
frame to the base frame it is seen again that eight parameters are required to transform from 
the world frame to frame 1, but a different set of parameters than found in path 1. In this 
simulation the second path is chosen. 

The tool transform is an Euler transform, requiring the specification of six 


parameters: 


Ae 7 rot(z,9e)rot(y,O6)rot(x,qa)trans(pxo. pyo. pzo) (32) 


-- 


2. Full Model of the PUMA, World Coordinate Frame, and 
Coordinate Measuring Machine 
Figure 20 shows the full pose calibration apparatus. The operation of the 


coordinate measuring machine will be explained in the experimental section of this chapter. 


37 





Figure 20. Full Pose Calibration Apparatus: PUMA, World Coordinate 


Frame and Coordinate Measuring Machine 


Table 2 represents the kinematic parameters of the full pose model [Ref. 10]. In 
the full pose model all thirty parameters not previously defined to be zero, indicated in 
boldface type, are able to be determined. Note the transform from the world coordinate 


frame to the base frame of the PUMA. 
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TABLE 2. NOMINAL KINEMATIC PARAMETERS FOR THE FULL 
POSE CALIBRATION 
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B. SIMULATION 
]. The Suite of Programs Used and the Strategy Involved 

In performing a simulation study of a proposed experimental calibration 
procedure the objectives are to: 

1. Confirm that the numerical algorithm proposed for the identification 
converges to the correct values. 

2. Predict the number of experimental poses required to identify the kinematic 
parameters to a defined degree of accuracy. 

3. Estimate the resulting accuracy of the manipulator if the new kinematic 


model was embedded in the control software of the manipulator [Ref. 11]. 
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Several computer programs were written to perform the simulation. These 


programs. and their relationships to each other are shown in Figure 21. 










PUMA-VAR.DAT 
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Figure 21. Flowchart for Full Pose Simulation 





The complete simulation may be regarded as a tool to plan the experiment in 
which the independent variables are the number of observations and the range of joint 
angles allowed by the common work volume of the PUMA and the coordinate measuring 


machine, while the dependent variables are the accuracy of the parameter identification and 


40 


the resulting manipulator accuracy [Ref. 12]. A detailed explanation of each program 
follows. 
a. The program JOINT 

Program JOINT does not require an input data file. 

Program JOINT generates sets of six joint angles which comprise a 
simulated pose of the PUMA. The program can be modified such that the simulated 
working volume of the PUMA is restricted from the maximum possible to either one-half 
of one-quarter of the maximum volume. 

The joint angles are selected using a random number generator. For 
instance, if the working volume of the PUMA has been selected as the maximum possible, 
the program takes the difference between the extreme joint angles possible, +180-(- 
180)=360, then multiplies this value by a number between 0 and 1, which has been 
generated randomly, thus generating one joint angle. Since six independent joint angles are 
required, six different random numbers and six different joint angles are generated for each 
pose, or observation. 

The output of program JOINT is a data file, PUMA-VAR.DAT. PUMA- 
VAR.DAT will have n sets of six joint angles, which comprise n simulated poses of the 
PUMA. In the verification phase program JOINT is run again, with the output data file 
being renamed POSEVER.DAT for use in program VERIFY. 

b. The program POSE 

Program POSE requires two input data files, PUMA-VAR.DAT, the 
output data file from program JOINT, and INPUT.DAT, the nominal kinematic parameters 
of the PUMA, listed in Table 1. POSE also inputs values "dangle" and "dlenth." The 
value assigned to "dangle" is added to all of the angular parameters except for the ones 


which are defined to be zero. The value assigned to "dlenth" is added to all of the length 
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parameters. Adding these values creates a manipulator which is significantly different from 
the manipulator reflected in the nominal kinematic parameter table, and is equivalent to 
supplying the kinematic identification program with pseudo-experimental data. Creating 
this different manipulator tests the integrity of the kinematic parameter identification 
program. Since the initial guess of the parameters will be the nominal parameters the 
identification program must rigorously solve for the actual kinematic parameters, which 
will be the nominal parameters plus "dangle" and "dlenth" as appropriate. 

Program POSE generates a T6 matrix, representing the simulated position 
and orientation of the tool frame of the PUMA, for each set of joint angles which were 
generated by program JOINT. 

When the joint angles are read from PUMA-VAR.DAT they are added to 
the parameters read from the nominal kinematic table which apply to the individual joint. 
For example, in POSE the variable TH1 equals the sum of DT1 and THETA]. DTI is 
input from the nominal kinematic table and THETA1 has been generated by JOINT. The 
nominal kinematic parameters are added to the simulated actual joint angles as any non-zero 
THETA value at any joint represents a movement from the zero position of that joint. 

Once the joint angles are calculated and the nominal kinematic parameters 
are adjusted, a forward kinematic solution is calculated. This will produce one T6 matrix 
for each set of joint angles. In order to accurately model an actual measurement in the 
laboratory noise is added to the position vector of the T6 matrix and the six joint angles of 
each simulated pose. The position noise arises from the uncertainty of the precision with 
which the coordinate measuring machine and associated software is capable of identifying 
the position of the center of a tooling ball. The position noise is generated by multiplying a 
random number which is generated in the same manner as in JOINT by a number read from 


INPUT.DAT, "magnx." The joint angle noise, called the encoder error, is generated by 


multiplying a random number by another value which is read from INPUT.DAT, "magnl." 
The position noise is 100 times the magnitude of the encoder noise. 

The output of POSE will be the six joint angles for each simulated pose 
and the corresponding T6 matrix for each set of joint angles. This data is stored in the 
output file PUMA-POS.DAT. 

c. The program ID6 

Program [D6 requires two input files, INPUT.DAT and PUMA- 
POS.DAT. The nominal kinematic parameters are read from INPUT.DAT in order to 
provide the kinematic parameter identification with a beginning point. The T6 matrices 
contained in PUMA-POS.DAT which were generated by program POSE contain the 
simulated poses with which the actual kinematic parameters will be identified in this 
program. 

ZXSSQ will perform the kinematic parameter identification. The function 
to be minimized by ZXSSQ 1s actually an array of six functions. Each pose will have six 
functions to be minimized, so if there are ten poses, or observations, ZXSSQ will have 
sixty functions to minimize using thirty variables. 


A subroutine within ZXSSQ, PUMA ARM, calculates the T6 matrix 
corresponding to the current kinematic parameters for each set of six joint angles input from 
PUMA-POS.DAT. The simulated measured Tg matrix read from PUMA-POS.DAT for 
each set of joint angles is known. Since the original nominal kinematic parameters were 
altered before the simulated measured Tg matrix was calculated, the simulated measured T6 
matrix will be different from the Tg matrix calculated using the generated joint angles and 
the nominal kinematic parameters. Also, the added measurement noise is present, 
providing more of a difference in the two matrices. For each observation the array of six F 


functions is calculated. After performing the matrix subtraction: 
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AT, = Tmeasured, >. Veaiculated. (33) 


the six F functions are determined for each observation: 


ΕΙ = AT(1,4) 

F2 = AT(2,4) 

F3 = AT(3,4) 
F, = ο ος AED) x scaling factor 
Fs = κ ο Ate) x scaling factor 
Ες Ξ ATED- A2 x scaling factor 


(34) 


The scaling factor which multiplies the functions in equation (34) is used 
to remove the emphasis from the position error. The position error in a pose will be much 
larger than the orientation error between two T6 matrices. For this reason the emphasis in 
the optimization program will be on eliminating the position error, while largely ignoring 
the orientation error. This is unacceptable, since the orientation error is equally important 
as the position error. The scaling factor places the orientation error at the same level of 
magnitude as the position error. 

The F functions in equation (34) represent the error matrix [Ref. 13]: 

0 -0, ὃν d, 
ô 0 -ô d 
ὃν ô 0 ὦ 


L0 0 0 0| (35) 


Ex 


It must be noted that equation (35) can only be used when the two 
matrices being subtracted are almost equal. In an actual experimental process the measured 


poses usually significantly differ from the predicted pose. This difference causes the A 
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matrix, equation 35, to have relatively large elements. ZXSSQ is often unable to converge 
under these conditions. The different version of ID6 will be introduced in the Experimental 
section of this chapter. 

After all the observations have been calculated for each pose, ZXSSQ 
compares each F value with a user defined convergence criteria. Convergence is defined 
when the kinematic parameters selected from one iteration to the next agree to four 
significant digits. If every F for each pose is less than this convergence criteria, then the 
current kinematic parameters are saved as the correct parameters. If not, ZXSSQ changes 
the thirty kinematic parameters and performs the process again until the convergence criteria 
Is satisfied. 

The output of ID6 is RESULT.DAT. The identified kinematic parameters 
will be stored in the same format as INPUT.DAT. In the simulation the identified 
kinematic parameters will be the parameters of INPUT.DAT plus dangle for orientation 
parameters and dlenth for length parameters plus some small error value created by the 
addition of the simulated measurement noise. 

d. The program VERIFY 

Program VERIFY requires three input programs. The first, 
POSEVER.DAT, is the output from program JOINT, and includes sets of six joint angles. 
The other two are kinematic parameters, INPUT.DAT and RESULT.DAT. INPUT.DAT, 
as explained earlier, contains the nominal kinematic parameters, and RESULT.DAT 
contains the kinematic parameters identified in program ID6. 

VERIFY measures the position and orientation difference between poses 
calculated using the nominal kinematic parameters and the nominal kinematic parameters. 
For each set of joint angles, VERIFY calculates a forward kinematic solution using both the 


nominal and identified kinematic parameters and takes the difference between the 
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corresponding T6 matrices. The position error and orientation error for all of the sets of 
joint angles are added and these values are divided by the total number of sets of joint 
angles. This will produce the total position error and total orientation error for a given 


number of observations. 


C. EXPERIMENTATION 
1. The Tooling Ball End Effector 

The tooling ball end effector is illustrated in Figure 22. Its’ purpose is to 
provide a means for determining the position and orientation of frame 6 of the PUMA. The 
five balls attached to the effector are precision tooled to a radius of 6.35 mm. This known 
radius, along with the known radius of the touch probe of the coordinate measuring 
machine provides the essential information required for identification of the center of each 
tooling ball by the routine BALL described earlier in the thesis. Also, each of the four 
tooling balls contained in the plane normal to the axis of the effector are nominally 90 
degrees apart, creating orthogonality between any two in series around the circumference of 
the tool. The fifth tooling ball's center lies on an axis which passes the two lines which 
attach balls on opposite sides of the tool, creating another orthogonality between the fifth 
ball and each of the other four. The orientation of frame six is fixed on the tool and is 
required information for the determination of the position and orientation of the tool frame 


by program CMMPOSE. 
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Figure 22. Tooling Ball End Effector 


2. The Coordinate Measuring Machine and World Coordinate Frame 
a. Construction 
The coordinate measuring machine (CMM) used for data acquisition in 


this thesis is illustrated in Figure 23. 
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Figure 23. Coordinate Measuring Machine 


The x-axis lies on the base of the CMM. Rotating the x wheel will move 
the x-axis receptacle to the left or right, changing the x-value displayed. The y-axis is 
contained within a post which is attached to the slide that changes the x position. Rotating 
the y wheel moves the y-axis receptacle up or down, changing the y-value displayed. The 
z-axis is contained is the y-axis receptacle assembly. Rotating the z wheel moves the z 
receptacle in or out, changing the z value displayed. Consider the point (1,1,1) relative to a 
known world reference frame. Rotating the x wheel will produce a change in position 


(Ax,1,1). Rotating the y wheel will produce a change in position (1,Ay,1). Rotating the z 
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wheel will produce a change in position (1,1,Az). It is clear that any point in the working 
volume of the CMM may be reached by the touch probe. 

A significant advantage of this CMM is its' ability to zero at any position. 
Each axis may be zeroed independently or concurrently. This is important in identifying 
the location of the world coordinate frame. A diagram of the world coordinate frame used 


in the full pose calibration is shown in Figure 24. 





position of 
world coordinate frame 


aspect of viewer 


Figure 24. The Full Pose World Coordinate Frame Cube 
The cube itself does not actually contain the world coordinate frame. 


Using the three faces of the cube which form the corner of the bottom of the cube closest to 


and most left of the operator, also identified in Figure 24, allow for a procedure which 
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identifies point (0,0,0) of the world coordinate frame. Once this point is identified, the 
axes of the world coordinate frame match the axes of the CMM. The procedure for 
identifying the world coordinate frame is as follows. Note that the order of the 
identification process of the world coordinate frame is arbitrary. 

1. The touch probe is positioned in area near the bottom, left, forward 
face of the cube. 

2. Activate the 'x-zero' mode of the CMM. Move, usa all three axes 
if necessary, the touch probe such that its' position is as close to the forward corner of the 
bottom of the left face as possible. Slowly move the probe to the right, using the x-axis 
wheel only, until contact is made with the forward face of the cube. When contact is made 
the display unit (DU) will beep and the x position will indicate zero. 

3. Activate the 'y-zero' mode of the CMM. Move, using all three axes 
if necessary, the touch probe such that its' position is as close to the forward, left corner of 
the bottom face of the cube as possible. Slowly move the probe upwards, using the y-axis 
wheel orily, until contact is made with the bottom of the cube. When contact is made the 
DU will beep and the y position will indicate zero. 

4. Activate the 'z-zero' mode of the CMM. Move, using all three axes 
if necessary, the touch probe such that its’ position is as close to the bottom, left corner of 
the forward face of the cube as possible. Slowly move the probe towards the cube, using 
the z-axis wheel only, until contact is made with the forward face of the cube. When 
contact is made the DU will beep and the y position will indicate zero. 

Once this procedure is completed a point in space located a distance equal 
to the diameter of the touch probe along each axis, (Xaia lY dias |2diag) will be identified as 


the location of the world coordinate frame. This process is easily repeatable and has a high 
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degree of accuracy, so for different sets of experimental measurements the position of the 
world coordinate frame should not vary to a significant degree. 
b. Data Acquisition Using the Coordinate Measuring Machine 

Data acquisition using the coordinate measuring machine consists of a 
repetitive measurement of the position of three different tooling balls on the end effector 
illustrated in Figure 22. The PUMA is operated in joint mode, which allows each of the six 
joints to be rotated independently. The end effector, which is attached to the tooling ball 
assembly, is moved to a variety of positions. When the operator is satisfied with the 
position of the tool, the measurement process begins. 

Execution of the pose measurement program, CMMPOSE, begins by 
inquiring which tooling ball on the tool is being measured. This information is required in 
order to determine the orientation of frame 6. The touch probe of the CMM is moved 
slowly towards the selected tooling ball. As the probe touches the surface of the ball the 
display unit of the CMM beeps. The position displayed by the unit is the position of the 
probe at the first instant that the probe touches the surface of the ball, and is displayed in 
millimeters to two significant digits. If the probe slightly slides after initial contact the 
displayed position will not change, remaining until one of the three axes of the CMM is 
moved. CMMPOSE asks for three touch probe measurement sets per ball and determines 
the center of each tooling ball using the procedure described in section Π.Β.2. Once the 
center of each ball is calculated a residual error reflecting the uncertainty of the accuracy of 
the predicted position of the center of the ball is displayed. An option of storing or purging 
the data is given at this point. Based on the judgement of the operator, the data is sent for 
further manipulation by CMMPOSE, or the data is purged and another set of ball 
measurements is taken. An acceptable residual error has a value near 0.000001 mm. Once 


the center of three of any of the five tooling balls, P1, Po, and P3, is determined, the 


E 


program synthesizes the position of the center of a fourth ball, P4, by calculating the vector 


cross product [Ref. 14]: 


P4 2 (P5 - Pj) x (P2 - P3) (36) 


The relationship between the coordinates of the center of each ball 


expressed in terms of the tool frame and the world coordinate frame is 


} 
1η 


[P; |= [A] [Pi (37) 


where Pe is the 4x1 column vector of the coordinates of the ith ball 
expressed with respect to the world frame, Pj is the 4x] column vector of the coordinates 
of the ith ball with respect to the tool frame, and A is the 4x4 homogeneous transformation 
matrix from the world frame to the tool frame. 

If Pj is known by precalibrating the tool. and Pi is measured then A may 
be calculated and used as the measured pose in the calibration process. The difficulty arises 


in inverting equation (37) to obtain A. The following information is known: 


(Pij = (AL Py. 
Fox! 


Ρ2-|Α..Ρ; 
P5 - [AYP3) 
[P4] - [A1(P4] (38) 


Algebraic manipulation produces: 


Co o sIr ος λα EE sm za UT E 
LP) Pai Ps, Pa) = Age oe ee 


(39) 


reducing: 


[pt lo | Any 
[P | =A, \P, (40) 


Since P. A, and P are all 4x4 matrices, equation (40) can be inverted, 


producing matrix A, the pose of the tool: 
[A] - P ]| P! (41) 
Two operators can expect to measure one pose in ten minutes. One 
operator will be utilized to move the PUMA and operate the CMM and the other operator 
will input data as instructed by program CMMPOSE. 
c. Identification of Actual Kinematic Parameters 
A modified version of program ID6 is used for the identification of the 
actual kinematic parameters of the PUMA. The modification is in the functions which are 
minimized by the IMSL routine ZXSSQ. As mentioned earlier, the delta matrix of equation 
(35) is suitable for use only when the predicted pose and the measured pose are quite 
close. Experimentation will usually produce matrices which are dissimilar. For this 
reason, each element of the matrix resulting from the difference between the predicted and 


measured pose is minimized: 


Meta. FF, Fi! | 811 312 2813 844 | |, 831 812 813 814 
EN Fs; Fe Fu -—| 82; 822 423 3824 ; _ 321 2822 38323 23824 
me Fe Fo Fi fot αλλα ο σον. 431 432 433 814 


3 
0 0 0 Ἱ πιεαςυτοά 


(42) 


0 0 0 IE c. 0 0 0 1 predicted L 


This produces twelve functions which are to be minimized. This method 
provides a more rigorous method for the identification of the kinematic parameters. Table 3 
shows the kinematic parameters identified using the full pose calibration procedure [Ref. 


15]. 


TABLE 3. FULL POSE IDENTIFIED PARAMETERS 
Nominal Value Identified Value 
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IV. PARTIAL POSE CALIBRATION 


A. THEORY 
1. Introduction 

As stated earlier, the nominal coordinate transformations from the base frame of 
the manipulator to frame 5 will not change. These transformations are inherent to the 
manipulator and are independent of the pose measurement scheme being employed. The 
transformations from the world coordinate frame to the base frame and from frame 5 to 
frame 6 are the same transformations as employed in the full pose calibration. 

The world coordinate frame is not in the same position, and is not defined in the 
Same manner, as it was in the full pose calibration. The partial pose calibration 
measurement system uses a linear slide to which the tool frame of the manipulator is 
attached. The only variable which is measured is the position of the slide relative to a zero 
position defined at the beginning of each pose measurement session. The world frame is 
defined as the T6 matrix of the tool when the slide's position on the slide equals zero. The 
slide acts as a prismatic joint and hence is defined only in direction. The location of the 
axis is undefined. The three translational components of the transform from frame five to 
frame six, T$. are arbitrary and may be set equal to zero. This defines that the origins or 
frame five and six are coincident, as shown in Figure 25. This will reduce the full thirty 
parameter model to twenty-seven parameters. 

The world coordinate frame's orientation is not known, either. The direction of 
one axis is able to be defined. In this experiment the axis of the linear slide has been 


defined as the x axis. The y and z axes of the world coordinate frame are orthogonal to the 


5D 


X-axis, but their orientation relative to the x-axis is undefinable, arbitrary, and therefore set 


equal to zero. This further reduces the model to twenty-six parameters. 
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Figure 25. Linear Slide Transformations 


The partial pose calibration model dictates that the too] frame pose is invariant. 


Each pose will have the following orientation: 
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This T6 matrix defines the x-axis of the tool frame to be in the same direction as 
the x-axis of the world coordinate frame, the y-axis of the tool frame to be in the same 
direction as the z-axis of the world coordinate frame, and the z-axis of the tool frame to be 


in the same direction as the negative y-axis of the world coordinate frame. Also, the 
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position of the origin of frame six is at position (X,0,0), where X is the position of the 
linear slide. Note that it is still not possible to uniquely define the orientation of the world 
coordinate frame with respect to any frame on the PUMA. It is only known that the 
orientation of the world coordinate frame is consistently defined with respect to the 
Orientation of the tool frame. 
2. Full Model of the Puma and the Linear Slide 

Figure 26 shows the partial pose calibration apparatus. The operation of the 

coordinate measuring machine as a linear slide will be explained in the experimental section 


of this chapter. 





Figure 26. Partial Pose Calibration Apparatus: PUMA and Linear Slide 
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Table 4 displays the kinematic parameters of the partial pose model. In the 
partial pose model twenty-six parameters are able to be determined. The parameters in 
boldface type are unable to be experimentally determined consequently their value is 
defined as equaling zero, or, in the case of the four parameters in the sixth transformation, 
the parameters are not independent, as explained in the previous section. Note the 
transform from the world coordinate frame to the base frame of the PUMA has changed 
from the full pose calibration. The world coordinate frame is not at the corner of the cube, 
as it was in the full pose calibration. The nominal kinematic parameters within the PUMA 


do not change, regardless of the type model used in the calibration process. 


TABLE 4. NOMINAL KINEMATIC PARAMETERS FOR THE PARTIAL 
POSE CALIBRATION 
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B. SIMULATION 
1. The Suite of Programs Used and the Strategy Involved 

As stated earlier, the simulation study is performed to: 

1. Confirm that the numerical algorithm proposed for the identification 
converges to the correct values. 

2. Predict the number of experimental poses required to identify the kinematic 
parameters to a defined degree of accuracy. 

3. Estimate the resulting accuracy of the manipulator if the new kinematic 
model was embedded in the control software of the manipulator. 

In the partial pose calibration, the forementioned "correct" values are the 
kinematic parameters identified using the full pose method. The computer programs 


written, and their relationships, are illustrated in Figure 27. 






PUMA-POS.DAT 


ID6. linsc 
RESULT.DAT 


Figure 27. Flowchart for Partial Pose Simulation 
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The complete simulation may be regarded as a tool to plan the — in 
which the independent variables are the number of observations and the range of joint 
angles allowed by the common work volume of the PUMA and the linear slide, while the 
dependent variables are the accuracy of the parameter identification and the resulting 
manipulator accuracy. A detailed explanation of each program follows. 

a. The program LINSC 

Program LINSC requires the input file INPUTDAT, the nominal 
kinematic data for the PUMA, which are listed in Table 1. LINSC also inputs values 
"dangle" and "dlenth," as did program POSE in the simulation phase of the full pose 
calibration. It will be instructive to reexamine the function of these two parameters. The 
value assigned to "dangle" is added to all of the angular parameters except for the ones 
which are defined to be zero. The value assigned to "dlenth" is added to all of the length 
parameters. Adding these values creates a manipulator which is significantly different from 
the manipulator reflected in the nominal kinematic parameter table, and is equivalent to 
supplying the kinematic identification program with pseudo-experimental data. Creating 
this different manipulator tests the integrity of the kinematic parameter identification 
program. Since the initial guess of the parameters will be the nominal parameters the 
identification program must rigorously solve for the actual kinematic parameters, which 
will be the nominal parameters plus "dangle" and "dlenth" as appropriate. 

Program LINSC generates six joint angles which will produce a T6 
matrix with an orientation that is predefined and fixed and a position on the slide which has 
been randomly generated. This is done by performing an inverse kinematic solution, since 
the T6 matrix is known. The set of joint angles, when used with the nominal kinematic 
parameters of the PUMA, which will produce the required T6 matrix is the required data. 


The IMSL routine ZXSSQ is again used to make this determination. 
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ZXSSQ uses an initial guess of all six joint angles equaling zero. A 
forward kinematic solution is calculated based on these joint angles and the nominal 
kinematic parameters plus dangle and dlenth. The difference between each element of the 
calculated T6 matrix and the required T6 matrix is calculated. These elements represent the 


functions to be minimized: 


ΕΙ Fa Fy Ειο] | 411 812 813 34 | | 811. 812. 813 814 | 
ETT; Fs Fi | a21 822 823 824 i 821 822 4823 824 | 
fm Feo Fo Γι; | 831 312 333 834 | 2500330 T 33380334 

L 0 0 0 1 3$ L 0 0 0 1 predicted L 0 0 0 l measured 


(44) 


ZXSSQ modifies the joint angle set until a convergence criteria is 
satisfied. At convergence the required T6 matrix and the six joint angles are returned to the 
main program. 

it has been stated that in order to accurately model an actual measurement 
in the laboratory noise must be added to the simulated T6 matrix and the six joint angles of 
each pose. The position noise and encoder noise is generated in the same manner as in 
program POSE in the full pose simulation. The position noise in the partial pose simulation 
arises from the uncertainty in the measurement of the position of the slide. The orientation 
error arises from inconsistencies in the bar supporting the slide. The bar almost certainly 
will have elements of twist and bend in it. These elements will cause differences in 
orientation as the slide moves along its' length. 

After the noise is added to the T6 matrix and the joint angles, the data is 
stored in the output file PUMA-POS.DAT. 

b. The program ID6 LINSC 

Program ID6 linsc requires two input files, INPUTLDAT and PUMA- 

POS.DAT. The nominal kinematic parameters are read from INPUT.DAT in order to 
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provide the kinematic parameter identification with a beginning point. The T6 matrices and 
associated joint angles contained in PUMA-POS.DAT which were generated by program 
LINSC contain the simulated poses with which the actual kinematic parameters will be 
identified in this program. 

ZXSSQ will perform the kinematic parameter identification. Each element 
of the matrix resulting from the difference between the predicted and measured pose is 
minimized, as was done during the actual kinematic parameter identification of the full pose 
calibration method. See equation (43) for the equation of the functions to be minimized. 

This produces twelve functions which are to be minimized. Subroutine 
PUMA ARM calculates the Te matrix corresponding to the current kinematic parameters 
for each set of six joint angles input from PUMA-POS.DAT. The simulated measured Tg 
matrix read from PUMA-POS.DAT for each set of joint angles is known. Since the 
original nominal kinematic parameters were altered before the simulated measured Tg 
matrix was calculated, the simulated measured Tg matrix will be different from the T6 
matrix calculated using the generated joint angles and the nominal kinematic parameters. 
Also, the added measurement noise is present, providing more of a difference in the two 
matrices. 

After all the observations have been calculated for each pose, ZXSSQ 
compares each F value with a user defined convergence criteria. Convergence is defined 
when the kinematic parameters selected from one iteration to the next agree to four 
significant digits. If every F for each pose is less than this convergence criteria, then the 
current kinematic parameters are saved as the correct parameters. If not, ZXSSQ changes 
the twenty-six available kinematic parameters and repeats the process until the convergence 


criteria is satisfied. 
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The output of ID6_linsc is RESULT.DAT. The identified kinematic 
parameters will be stored in thc same format as INPUT.DAT. In the simulation thc 
identified kinematic parameters will be the parameters of INPUT.DAT plus dangle for 
orientation parameters and dlenth for length parameters plus some small error value created 


by the addition of the simulated measurement noise. 


C. EXPERIMENTATION 
1. The Coordinate Measuring Machine as a Linear Slide 
a. Construction 
The coordinate measuring machine's base, which supports the x-axis, is 
mounted on an incline, as illustrated in Figure 28. The incline is placed on the table on 
which the PUMA is mounted, in an orientation which is significantly different from the x- 
axis of the PUMA. This position, concurrent with the inclined mounting, provides for the 


maximum possible joint rotation of all six joints of the PUMA. 


N 
t 





Figure 28. Coordinate Measuring Machine as Linear Slide 


b. Data Acquisition Using the Linear Slide 
Data acquisition using the linear slide is as follows. The PUMA is fixed 
to the slide using a plate fitted with holes to facilitate attachment to both joint six and the 
linear slide. The mounting plate has an offset which allows the kinematic parameters 
associated with the sixth transform to be identified. The PUMA is placed in "Free" mode, 
allowing unimpeded rotation of each joint, while continually being supported by one 
member of the measurement team. The PUMA is quite heavy and great care must be taken 


to prevent abrupt movement of the manipulator arm while joint six is attached to the slide. 
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The slide is moved to one end of the slide and the x-axis position indicator on the display 
unit is zeroed. The slide should be positioned at the lower end of the slide when being 
zeroed in order to have positive x-axis position indications on the display unit. The slide is 
moved up the scale in incremental distances, and after each increment the six joint angle 
positions of the PUMA and the x position of the slide is input using program ACDAT. The 
position which has been input is stored with the orientation matrix, which will not change, 
regardless of the position of the slide. 

Two operators can expect to measure one pose in one minute. One 
operator is utilized to move and support the PUMA while the other will input the position 
of the slide and the joint angles of the PUMA, as directed by program ACDAT. 

c. Identification of Actual Kinematic Parameters 

The same version of program [D6 linsc which was used in the 
identification of the kinematic parameters in the simulation phase of the partial pose 
calibration is used in the identification of the actual kinematic parameters. Table 5 shows 


the kinematic parameters identified using the full pose calibration procedure. 
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TABLE 5. PARTIAL POSE IDENTIFIED PARAMETERS 


Parameter Nominal Value Identified Value 
OD 151.0 150.4016 
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V. DISCUSSION OF RESULTS 


A. GENERAL OBSERVATIONS 

Both the full pose and partial pose calibration methods successfully provided 
kinematic parameters which could be used in a control program to use the PUMA most 
accurately. Each simulation predicted convergence to a unique set of kinematic parameters, 
thirty for the full pose method and twenty-six for the partial pose method. Poses measured 
in the laboratory provided the data which proved this prediction. 

The results of the kinematic parameter identification using the full pose calibration 
method indicates that the resulting accuracy of the manipulator is 0.3 mm. The 
experimentally predicted accuracy was considerably larger than the predicted accuracy from 
the simulation, which was approximately 0.15 mm. This is most likely attributed to a 
higher magnitude of noise being present in the pose measurements than predicted in the 
simulation. It may be concluded that using this type of calibration method will improve the 
accuracy of the manipulator to about the same value of the manipulator repeatability [Ref. 
16]. 

The results of the kinematic parameter identification using the partial pose calibration 
method indicate that the resulting accuracy of the manipulator is 0.9 mm. It may be 
concluded that using this type of calibration method will provide a manipulator which is 
three times less accurate than a manipulator calibrated using the full pose method, but still 
more accurate than an uncalibrated manipulator. 

The method to determine accuracy of the partial pose calibration method 1s as follows. 
A total of 42 poses were measured, 21 in lefty configuration, 21 in righty. The lefty and 


righty configuration is a description of joint 1's position. If, while the operator is facing 
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the PUMA, joint 1 is rotated such that the axis of joint one points to the operator's right, 
the PUMA is in lefty configuration. The opposite holds for righty configuration. Figure 
29 shows the PUMA in lefty, elbow down configuration. Note the tooling ball end 


effector attached to joint six. 





Figure 29. The PUMA 560 in Lefty, Elbow Down Configuration 
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While all 42 poses were used in the identification of the kinematic parameters shown 
in Table 5, to determine the accuracy, the poses were divided. The first ten poses 
measured in the lefty and righty configuration were merged to form a set of twenty poses 
which were used in program ID6 LINSC to determine a set of identified kinematic 
parameters, an output file KINPARAM.DAT. The last ten poses measured in the lefty and 
righty configuration were merged into a data file, MEASURE.DAT, to act as the measured 
poses. The program FFOR was executed, calculating the predicted poses based on the 
actual joint angles of the PUMA, read from MEASURE.DAT, and the identified kinematic 
parameters, read from KINPARAM.DAT. The result is two sets of T6 matrices, one the 
actual measured T6 matrix, the other the predicted Tg matrix. Program COMPFOR 
calculates the RMS position difference and orientation difference between the two sets of 


T6 matrices. 


B. COMPARATIVE OBSERVATIONS 

Table 6 shows the identified kinematic parameters for the PUMA using the full pose 
calibration method and the partial pose calibration method. A comparison of the two 
methods of calibration described in this thesis, along with an analysis of the identified 


calibrated parameters, will follow. 
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TABLE 6. COMPARISON BETWEEN FULL AND PARTIAL POSE 
IDENTIFIED KINEMATIC PARAMETERS 


Parameter Nominal Value Identified Value Identified Value 
FULL POSE ARTIAL TN 
ΓΞ o a O| oo | voas | ο: ὋΝ 


-89.9977 -89.9778 
-0.4888 -0.5600 


| m | aw [| aom | 431.9559 —— 
|. — | 00 | ooe ME  . 
oe, | oo | «1249 | 1.0708 —— 
— a | mom | — 149145 | 149453 
| o | 900 | 900512 
m" ———M μα μα σί ΓΕ 
|. d, | ao | — 432889 | 433.0048 
| a | 
a ΤΏ | 599 | πότος 
|. 8€& | — 00 (| 2254 | 2313 
[Ld | oo (|  -.0629 | 0390 | 
[3 | oo | -00058 — | — 08460 —— 
|. ας 1. 900  — |! 8995 | 895300 



























Most robot manipulator arms will be used in an industrial environment. While it is 
important that the manipulator be properly calibrated, the amount of time the manipulator is 
unavailable for operation due to the calibration process being performed is significant. The 
experiments performed clearly have shown the full pose method takes considerably more 


time to accomplish than the partial pose method. The full pose method takes approximately 
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ten minutes to make one pose measurement, while the partial pose measurement takes only 
One minute to measure one pose. The amount of time required could be cut dramatically if 
an interface between the PUMA control computer and the data acquisition computer were 
built. This interface would eliminate the need for manual entry of the six joint angles for 
each pose. The relative difference in the amount of time required to measure one pose 
would still be present. 

The full pose calibration method provides for a more accurate calibration of the 
manipulator. This fact arises from two considerations. First, more of the kinematic 
parameters of the manipulator can be identified using the full pose method, thirty, than the 
partial pose method, twenty-six. A thirty parameter model is logically a more detailed 
model of the manipulator than a twenty-six parameter model. Second. the full pose 
calibration uses a working volume of the manipulator which is significantly larger than the 
working volume used in the partial pose method. In actuality, the area used in the full pose 
calibration is indeed a volume. The area used in the partial pose model is comprised of 
only a line which is slightly longer than 900 mm. This thesis did not examine the validity 
of the calibration of the PUMA once the PUMA operates outside the calibration volume. It 
seems logical that if the tool of the PUMA was placed a considerable distance from the 
region in which it was calibrated, the calibration becomes invalid. 

Another difference between the full and partial pose methods is the association 
between the number of poses required for a calibration. Each pose measured using the full 
pose calibration provides six units of information: the position, (x6. y6. z6). of the origin 
of frame 6, and the orientation of frame 6, the direction cosines of each coordinate axis. 
The full pose calibration determined thirty kinematic parameters. In the complete absence 
of noise five poses would be required to identify the kinematic parameters, since thirty 


unknowns require thirty units of information for a unique solution. Each pose measured 
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using the partial pose calibration provides two units of information, since the frame which. 
the pose is defined with respect to, is not known. Four units of information are required to 
define the frame, the sum of which is six units of information, as 15 available in the full 
pose calibration. In the complete absence of noise fourteen poses would be required to 
identify the kinematic parameters, since twenty-six unknowns require twenty-six units of 
information for a unique solution. 

The full pose calibration showed that it is very important to exercise all six of the 
joints as much as possible during the pose measurement phase. This requirement arises 
due to the presence of measurement noise [Ref. 17] The simulation's level of 
measurement noise was not as large as the noise determined to be present in the actual 
measurement phase. This allowed for a unique solution in the simulation without large 
Joint rotations. The measured pose calibration process showed the increased amount of 
noise being present, and it was determined that maximizing the joint rotation alleviated the 
presence of the measurement noise in the coordinate measuring machine and in the 
encoders within the joints of the PUMA. The PUMA was exercised by taking a series of 
poses which would transcribe a cube in front of the PUMA, in elbow up and elbow down 
(joint 3 is the elbow), and in lefty and righty configurations. 

The problem of exercising the joints resurfaced in the partial pose calibration. The 
slide's length and position relative to the PUMA prevented adequate exercising of the 
joints. If poses which were taken in only one configuration, i.e. lefty or righty, were used 
in a calibration attempt using ID6 LINSC, the identified kinematic parameters were found 
to be significantly different from those identified using the full pose method, and were 
disregarded as inaccurate. The presence of noise arising from the slide's flexure on its' 
mount dominated in this case. The first slide used in the partial pose calibration process 


had dramatic flexure, and was discarded in preference of the more stiff coordinate 


92 


measurement machine x-axis slide. Even with the reasonably stiff coordinate measuring 
machine it was shown that regardless of how many poses were measured in one 
configuration, an accurate calibration was not possible. To conduct an accurate calibration, 
a series of poses were measured along the length of the slide and then the PUMA was 
disconnected from the slide. Joint 1 was rotated such that the configuration was reversed 
from the lefty configuration to the righty configuration, the tool frame was reattached to the 
slide, and another series of poses was measured along the length of the slide. It was not 
possible to obtain poses in both the elbow up an elbow down configuration due to the 
proximity of the PUMA to the slide. All of the poses were taken in the elbow up 
configuration. Measuring poses in both the lefty and righty configuration countered the 
presence of the measurement noise, but the resultant accuracy of the calibration still did not 
match that of the full pose calibration. 

The orientation of the slide presented a problem as well. Two calibration attempts 
were attempted with the slide laying flat on the table on which the PUMA is mounted. It 
was found that the orientation of the slide prevented the joint rotation necessary to 
overcome the measurement noise present. The next attempt placed the slide on an incline of 
about thirty degrees. The slide/incline apparatus was first oriented nominally in line with 
the x-axis of the PUMA. Again, adequate joint rotation was not possible. The difficult 
joints to move were the wrist joints, joints 4 through 6. Also, there was a problem with the 
tool frame entering areas of singularity. The area is said to be singular if the tool frame can 
be restrained in one position while a joint is moved freely. Simply stated, this means a 
singular pose can be defined with several different sets of joint angles. To remove the 
problem of singularity and to achieve the required joint rotation, the slide/incline apparatus 


was placed at an angle from the x-axis of the PUMA. Using this orientation and the pose 





measurement procedure described in the 
- 
completed. 
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VI. CONCLUSIONS 


The results of the research reported in this thesis provide the following conclusions: 

1. It is possible to improve the accuracy of a PUMA 560 robot manipulator to 
significantly less than the current possible accuracy of 10.0 mm, and equal to the 
repeatability, 0.3 mm. 

2. Full pose calibration of the PUMA 560 is possible and will identify thirty 
kinematic parameters, producing an accuracy of 0.3 mm. 

3. Partial pose calibration of the PUMA 560 is possible and will identify twenty- 
six kinematic parameters, producing an accuracy of 0.9 mm. 

4. Obtaining the maximum possible joint rotation of all six joints is required for 
robust convergence to the identified kinematic parameters of the manipulator. 

5. Measuring poses in lefty, righty, elbow up, and elbow down configuration 
facilitates maximum joint rotation and corresponding robust convergence. 

6. The partial pose measurement phase showed that the orientation of the slide 
relative to the PUMA is of paramount importance in the identification of the kinematic 
parameters. 

7. More information per pose is determined using the full pose calibration, but the 
excessive time required, ten minutes per pose, detracts from the efficiency of the method. 

8. To use the partial pose method of calibration requires that the manipulator have 
a "Free" mode, in which each joint can be physically rotated. 

9. The "Delta" matrix, showed in equation 35 should not be used as the functions 
to be minimized in the calibration procedure. Each element of the T6 matrix should be used 


as functions to be minimized. 
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APPENDIX A. PROGRAM BALLI 


Program BALL 


This program takes four sets of Cartesian coordinates 
assumed to be from a CMM touching the surface of a precision tooling 
ball, and finds the center of the ball. 


integer n,m,nsig,maxfn,iopt,i,infer,ier,ixjac 

real*8 parm(4),x(3),£(4),xjac(4,3) ,work(29),eps,delta, 
xjt3(6),y(4,3) 

external resid 

common y 


m-4 

nz3 
ixjac=4 
nsig=3 
eps=0.0 
delta=0.0 
maxfn=500 
1opt=1 


c get touch data 


1000 


do 1=1,4 

write(6,*)'Move the CMM and type in data for point £',i 
read (5,*) y(i,1),y(i,2),y(i,3) 

enddo 


c set up start point for identification 


800 


x(1)=y(1,1) 
X(2)=y(1,2) 
x(3)7y(1,3) 


call zxssq(resid,m,n,nsig,eps,delta,maxfn,iopt,parm,x,ssq,f, 
xjac,ixjac,xjtj,work,infer,ier) 


write(6,800) 'center (x,y,2) is:',x(1),x(2),X(3) 
write(6,*) 'residual error is:', ssq 
format(1x,a,3f9.3) 


end 


subroutine resid(x,m,n,f) 


This subroutine calculates the residual functions used by the 
IMSL routine ZXSSQ, called from subroutine BALL. 


integer m,n 


real*8 x(n),f(m),y(4,3),res,re,r0 
common y 
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r0=7.85 


do 1=1,4 
res-(xM-y(1,1))99*00x(2) -y (1,2]y9*2* (x (3) -y(i,3))**2 
re-dsqrt(res) 
f(i)sabs(re-r0) 

enddo 

sum=0.0 

do i=1,4 
sum=sum-+ f (i) 

enddo 

write(6,*) sum 

return 

end 
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APPENDIX B. PROGRAM CMMPOSE 


C an um dium dium am am 4m diu diu» ----- ---------.-------------------------- ------------------------ ---------- 
Program CMMPOSE 

c This program accepts coordinate data from the CMM, together 

τ with the tooling ball i.d. and generates a 4x4 pose matrix 

e corresponding to the pose. Four measurements on each of three 

ο balls is required. 


parameter (lda=4, ldainv=4,n=4,1d3=3) 

Geared x(3,3),2(3),p(5,3),pa(3) ,pb(3),pe(3),pd(3),p4(4,4), 
& pd4(4,4),pinv(4,4),¢(4,4),sum,val,xl,yl,2l, 
& X2,y2,22 

integer id(3),flag 

character reply 

external wrrrn 


c Precalibration data for the tool (in column order) 
data p/0.0,50.740,0.0,-50.913,0.0, 
& 0.0,0.0,50.703,;0.0,-5079B87 
& 51.111,0:.0,0-070 0 59107 
c open data file 
open(9,name-'posex.dat' ,status-'new') 
c gather data from three balls 
1000 do j=1,3 


write(6,*) ‘Input ball reference number’ 
read(5,*) id(j) 


call ball(2) 
do k=1,3 
x(J,K)=z(k) 
enddo 
enddo 


c synthesize the fourth ball - measured 


do 151, 3 
pa(j)=x(1,3) 
pb(3)9x(2,2) 
pc(3)7x(3,3) 
enddo 


x1=(pb(1)-pa(1))/70.71 
yl=(pb(2)-pa(2))/70.71 
zl-(pb(3)-pa(3))/70.71 


X2-(pc(1)-pa(1))/70.71 


y2=(pe(2)-pa(2))/70.71 
z2=(pe(3)-pa(3))/70.71 
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pd(1)=(yl*z2-y2*z1)*70.71+pa(i) 
pd(2)=(x2*z1-x1*z2)*70.71+pa(2) 
pd(3)2(xl*y2-x2*y1)*70.71-4pa(3) 


c compose the PD4 matrix 


do i=1,3 

pd4(i,1)=pa(i) 
pd4(i,2)=pb(i) 
pd4(i,3)=pe(i) 
pd4(1,4)=pd(i) 


enddo 


do i=1,4 
pd4(4,i)=1.0 
enddo 


c synthesize the fourth ball - tool 


do j=1,3 
pa(j)=p(id(1),3) 
pb(j)=p(id(2),3) 
Pe(j)=p(id(3),3) 
enddo 


xl-(pb(1)-pa(1))/70.71 
yl=(pb(2)-pa(2))/70.71 
zl-(pb(3)-pa(3))/70.71 


x2-(pc(1)-pa(1))/70.71 
y2-(pc(2)-pa(2))/70.71 
Z22-(pc(3)-pa(3))/70.71 


pd(1)=(yl*z2-y2*z1)*70.71+pa(1) 
pd(2)2(x2*21-x1*22)*70.71-*pa(2) 
pd(3)2(xl*y2-x2*y1)*70.714pa(3) 


c compose the P4 matrix 


do 121,3 
p4(i,1)=pa(i) 
p4(i,2)=pb(i) 
p4(i,3)=pe(i) 
p4(i,4)=pd(1) 
enddo 


do i=1,4 
p4(4,i)=1.0 
enddo 
c calculate the T matrix 
call dlinrg(n,p4,lda,pinv,ldainv) 
call matmulc(t,pd4,pinv) 
call dwrrrn('T',n,n,t,ldàinv,0) 
c orthogonality and size check 
flag=0 


sum-0.0 
do j=1,3 


iW) 


sumsumtt(j,1)*t(j,2) 
enddo 

val=dabs (sum) 
write(6,*)'val-',val 
ἀξ(ναι σε. ο flag-I 


do j=1,3 
sum-sum*t (3,2) *t(3,3) 
enddo 

val=dabs (sum) 
write(6,*) 'val=',val 
if(val.gt.0.01) flag=1 


do j=1,3 
sum=sumtt(j,1)*t(j,3) 
enddo 

val=dabs (sum) 
write(6,*)'val-',val 
if(val.gt.0.01) flag-1 


do αι, 3 
sum-0.0 
do j21,3 
sum-sum*t (j,i)*t(j,i) 
enddo 
val=dabs (sum-1.00000) 
write(6,*)'val-',val 
if(val.gt.0.01) flag-1 
enddo 


if(flag.eq.1) then 

write(6,*) 'orthogonality test: FAIL' 
else 

write(6,*) 'orthogonality test: PASS' 
endif 


C save data to file 


do i=1,4 
write(9,700) t(i,1),t(i,2).t(2 ο. 
enddo 
700 format(4el7.8) 
write(9,*) 


c reminder to get joint angles 
write(6,*)'Type WHERE on the PUMA console' 
C need more data ? 
1020 write(6,*) 'More data...(y=yes, n=no)' 
read (5,'(a)') reply 
if (reply.eq.'y') goto 1000 


if (reply.eq.'n') goto 1010 
goto 1020 


1010 close (9) 
close(10) 
end 
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subroutine BALL(x) 


This program takes three or four sets of Cartesian coordinates 
assumed to be from a CMM touching the surface of a precision tooling 
ball, and finds the center of the ball. 


external resid 

integer n,m,nsig,maxfn,iopt,i,infer,ier,ixjac 

real*8 parm(4),x(3),£(4),xjac(4,3) ,work(29),eps,delta, 
x3t3(6),y(4,3) 

character reply 

common y 


open(10,name-'test.dat',status-'old') 


m=4 

n=3 
ixjac=4 
nsig=3 
eps-0.0 
delta=0.0 
maxfn=500 
lopt=1 


c get touch data 


1020 


do 1=1,4 

write(6,*)'Move the CMM and type in data for point #',i 
read (10,*)y(i,1),y(1i,2),y(i,3) 

read (5,*) y(i,1),y(i,2),y(i,3) 

ο ο ο ον Ty yr1,2),y(1,3) 

enddo 


c set up start point for identification 


800 


1000 


1010 


x(1)=y(1,1) 
x(2)=y(1,2) 
x(3)=y(1,3) 


call zxssq(resid,m,n,nsig,eps,delta,maxfn,iopt,parm,x,ssq,f, 
xjac,ixjac,xjtj,work,infer,ier) 


write(6,800) ‘center (x,y,z) 18:',x(1),x(2),x*(3) 
write(6,*) ‘residual error is:', ssq 
format(1x,a,3f9.3) 


write(6,*) 'Keep this data (y-yes, n-no)' 
read (5,'(a)') reply 

if (reply.eq.'y') goto 1010 

if (reply.eq.'n') goto 1020 

goto 1000 


return 
end 
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subroutine resid(x,m,n,f) 


δ] 


This subroutine calculates the residual functions used by the 
IMSL routine ZXSSQ, called from subroutine BALL. 


integer m,n 
real*8 x(n),f(m),y(4,3),res,re,r0 


common y 

r0=7.85 

do iz1,4 
res=(x(1)-y(i,1) )**2+(x(2)-y(1,2) ) **27( x3) syne 
re=dsqrt (res) 
f(i)sabs(re-r0) 

enddo 

gum-0.0 

do i=1,4 
sum-sum-f(i) 

enddo 

return 

end 
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APPENDIX C PROGRAM JOINT 


PROGRAM JOINT 


This program generates the six joint angles for the 
PUMA manipulator arm for the simulation of the PUMA's 
calibration using the coordinate measurement machine. 
The six joint angles are generated using a random number 
generator. 


INTEGER I, J, K, NOBS, MAXNOBS 
PARAMETER (MAXNOBS=360) 
REAL Q(MAXNOBS,6), QMIN(6), QMAX(6) 


COMMON /C1/ Q, QMAX, QMIN 
In this section the working volume of the PUMA can be selected. 
The working volume can be full, half, or one quarter, based on 
extent of the allowed joint movement. 

DATA QMIN/ -180.0, -180.0, -180.0, -180.0, -180.0, -180.0 / 

DATA, OMAX/ 180.0, 180.0, 180.0, 180.0, 180.0, 180.0 / 

WRITE (6,*) 'Volume is FULL' 

DATA QMIN/ -90.0, -90.0, -90.0, -90.0, -90.0, -90.0 / 

DATA QMAX/ 90.0, 90.0, 90.0, 90.0, 90.0, 90.0 / 

WRITE (6,*) 'Volume is HALF' 

DATA QMIN/ -45.0, -45.0, -45.0, -45.0, -45.0, -45.0 / 

DATA QMAX/ 45.0, 45.0, 45.0, 45.0, 45.0, 45.0 / 

WRITE (6,*) 'Volume is QUARTER' 
Open the output data file, PUMA-VAR.DAT. 

OPEN (8, NAME='PUMA-VAR.DAT', STATUS='NEW') 


Input the number of observations from the nominal kinematic 
data file, INPUT.DAT. 


OPEN (9, NAME-'INPUT.DAT', STATUS='OLD') 


DO I=1,10 
READ (9,*) 
ENDDO 


READ (9,*) NOBS 
CLOSE (9) 


Call the random number generation routine to obtain a set 
of six random joint angles. 


CALL MSPREAD (NOBS) 


Save the joint variable data in the output file, PUMA-VAR.DAT. 


83 
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DO II = 1, NOBS 
WRITE (8,*) Q(II,1),Q(II,2),Q(II, 3) , Q( II, 4) , Q( 11,5) , Q(11,6) 
ENDDO 


CLOSE (8) 
STOP 


END 


**SUBROUTINES** 


**SUBROULINE I** 
SUBROUTINE MSPREAD (NOBS) 


This subroutine generates the six required joint angles 
by the Monte Carlo method. The six joint angles are 
generated by using six independently random numbers. 


INTEGER I, J, K, NOBS, MAXNOBS 

PARAMETER (MAXNOBS=360) 

REAL Q(MAXNOBS,6), QMIN(6), QMAX(6), MAGQ(6), NUM 
INTEGER*4 ISEED 

COMMON /Cl/ Q, QMAX, QMIN 


Obtaining the random number seed. 


WRITE (6,*) 'Type in a 6-digit random number seed' 
READ (5,*) ISEED 


Calculating the scaling factor for each joint angle. 
DO I= 1, 6 
MAGOQ(I) = QMAX(I)-QMIN(I) 
ENDDO 
Generating the six joint angles. 
DO J = 1, NOBS 
DOI= 1, 6 
CALL RANDOM (ISEED,NUM) 
Q(J,I) = QMIN(I) * MAGQ(I) * NUM 
ENDDO 
ENDDO 
RETURN 


END 


**SUBROULINE T244 
SUBROUTINE RANDOM (X,2Z) 
REAL FM, FX, Z 


INTEGER A, X, I, M 
DATA I/1/ 
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1000 


IF ( I .EQ. 0 ) GO TO 1000 
I-0 

M- 2 ** 20 

FM- M 

A- 2**10 4 3 


X= MOD ( A*X ,M) 
FX- X 

Z= FX/ FM 
RETURN 


END 


85 


Ω  Ω Ω Ω 


Ω 


Ω Ω Ω Ω 


APPENDIX D PROGRAM POSE 


PROGRAM POSE 


This program generates a T6 matrix for each set of joint 

angles which were generated using the program JOINT. The T6 
matrix is calculated by performing a forward kinematic solution 
using the six joint angles which are read from PUMA-VAR.DAT 

and the nominal kinematic data which are read from INPUT.DAT. 


INTEGER*4 ISEED 
INTEGER I, J, K, NOBS, MAXNOBS, N 


REAL*8 RNX, RNY, RNZ, MAGNX, MAGN1, DANGLE, DLENTH 
REAL*8 RN1, RN2, RN3, RN4, RN5, RN6, PI 


PARAMETER (PI-23.141592653589793) 
PARAMETER (MAXNOBS-360) 


REAL*8 FIO, SIO, THO, PXO, PYO, PZO 

REAL*8 DT1, DT2, DT3, DT4, DT5 

REAL*8 DD1, DD2, DD3, DD4, Ρ05 

REAL*8 AA1, AA2, AA3, AA4, AAS 

REAL*8 ALI, AL2, AL3, AL4, AL5 

REAL*8 BL1, BL2, BL3, BL4, BL5 

REAL*8 FI6, TH6, SI6, PX6, PY6, PZ6, DF6 

REAL*8 THETA1, THETA2, THETA3, THETA4, THETA5, THETA6 
REAL*8 TH1, TH2, TH3, TH4, TH5 

REAL*8 T0(4,4), T1(4,4), T2(4,4), T3(4,4) 

REAL*8 T4(4,4), T5(4,4), T6(4,4), TRPY(4,4), TXYZ(4,4) 
REAL*8 TIMAT(4,4), T(4,4) 


Initializing the TIMAT matrix to an identity matrix. 
DATA TIMAT/1,0,0,0,07170/0,0,02 1409 0 OORE 


Obtain a random number seed. This random number will be used 
later in the program to generate random noise which will model 
an actual measurement using the coordinate measurement machine. 


WRITE (6,*) ‘Type in a 6-digit random number seed.' 
READ (5,*) ISEED 


Open the two input files, PUMA-VAR.DAT and INPUT.DAT, and 
the output data file, PUMA-POS.DAT, which will contain the 
six previously determined joint angles and the corresponding 
T6 matrix calculated in this program. 

OPEN (8, NAME-'PUMA-VAR.DAT', STATUS-'OLD') 

OPEN (9, NAME-'PUMA-POS.DAT', STATUS- NEW') 

OPEN (10,NAME-'INPUT.DAT', STATUS-'OLD') 
Reading nominal kinematic parameters from INPUT.DAT. 


read (10,*) 
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c 
c 
c 


c 
c 


read 
read 
read 
read 
read 
read 
read 
read 
read 
read 


(107 *) f105th07810,pxO,pyO,pzo 
(10,*) dtl,ddl,aal,all,bll 
(10,*) dt2,dd2,aa2,a12,b12 
(10,*) dt3,dd3,aa3,a13,b13 
(10,*) dt4,dd4,aa4,al4,bl4 
(10,*) dt5,dd5,aa5,al5,b15 


(10,*) 


(10,*) df6,th6,si6,px6,py6,pz6 


(10,*) 


(10,*) nobs,n,dangle,dlenth,magnx,magnl 


Adding encoder offsets and setting link parameters, creating 
a PUMA manipulator different from the manipulator represented 
by the nominal kinematic parameters. 


dti 
dt2 = 
dt3 
dt4 
dt5 


{10 Ξ 
tho = 
310 
px0 
py0 = 
pzo 


all 
al2 
al3 
al4 
al5 = 


aal = 
aa2 
aa3 
aa4 
aa5 = 


ddl = 
dd2 
dd3 
dd4 
dd5 = 


bll = 
bl2 
bl3 = 
bl4 
b15 


df6 
th6 
816 
px6 
ΡΥ6 
pz6 


- dtl 
dt2 + dangle 
dt3 + dangle 
dt4 + dangle 
dt5 + dangle 
fiO + dangle 
tho + dangle 
si0 + dangle 
px0 + dlenth 
pyO + dlenth 
pz0 + dlenth 
all + dangle 
al2 + dangle 
al3 + dangle 

= al4 + dangle 
al5 + dangle 
aal + dlenth 
aa2 + dlenth 
aa3 + dlenth 

= aa4 + dlenth 
aa5 + dlenth 
ddl 

0.0 


dd3 + dlenth 
dd4 4 dlenth 
dd5 + dlenth 


b11 

- bl2 4 dangle 
b13 
bl4 
b15 
df6 + dangle 
thé + dangle 
si6 + dangle 
px6 * dlenth 
py6 + dlenth 
pz6 + dlenth 


| defined 


defined 


| defined 
! defined 
| defined 


Loop through the program, generating a T6 matrix for each 
set of 6 joint angles. 


The number of loops will be reflected 
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c 


c 


0000 


by the entry "nobs" in the nominal kinematic parameter file, 
INPUT.DAT. 


DO I = 1, NOBS 
Initializing the T matrix to an identity matrix. 
DO J=1,4 
DO K=1,4 
T(J,K) * TIMAT(J,K) 


ENDDO 
ENDDO 


Read the sets of six joint angles from the data file PUMA-VAR.DAT. 


READ (8,*) THETA1, THETA2, THETA3, THETA4, THETAS, THETA6 


TH1 = DT1 + THETAI 
TH2 = DT2 + THETA2 
TH3 = DT3 + THETA3 
TH4 = DT4 + THETA4 
TH5 = DT5 + THETAS 
FI6 = DF6 + THETA6 


Computing the six individual T matrices, Tl thru T6. 


T3RPY ( FI6, TH6, SI6, TRPY ) 
T3XYZ ( PX6, PY6, PZ6, TXYZ ) 
MATMULC ( T6, TRPY, TXYZ ) 


CALL T3RPY ( FIO, THO, SIO, TRPY ) 

CALL T3XYZ ( PXO, PYO, PZ0, TXYZ } 

CALL MATMULC ( TO, TRPY, TXYZ ) 

CALL TRANSFORM ( ALl, AA1, 95051, ΤΗ], BL1, ΤΊ ) 
CALL TRANSFORM ( AL2, AA2, DD2, TH2, BL2, T2 ) 
CALL TRANSFORM ( AL3, AA3, DD3, TH3, BL3, T3 ) 
CALL TRANSFORM ( AL4, AA4, DD4, TH4, BL4, T4 ) 
CALL TRANSFORM ( AL5, AA5, DD5, TH5, BL5, T5 ) 
CALL 

CATE 

CALI 


Computing the overall transformation, T. 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, ΤΙ ) 
CALL MATMULA ( T, T2 ) 
CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, T5 ) 
CALL MATMULA ( T, T6 ) 


Generating the random noise. This noise will be added to 
the theoretical measurement data and the encoder readings 
to accurately simulate the actual measurements which will 
be made in the lab. 


CALL RANDOM(ISEED,RN1) 
CALL RANDOM(ISEED,RN2) 
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CALL RANDOM( ISEED, RN3) 
CALL RANDOM( ISEED, RN4) 
CALL RANDOM( ISEED, RNS) 
CALL RANDOM( ISEED, RN6) 


RNX = 
RNY 
RNZ = 


MAGNX*( 
MAGNX*( 
MAGNX*( 


2.0*RNX - 1 
2«Ü0*RNY - 1. 
2.0*RN2 - 1 


RN1 = 
RN2 = 
RN3 = 
RN4 = 
RN5 = 
RN6 = 


MAGN1*( 2.0*RN1 - 1 
MAGN1*( 2.0*RN2 - 1 
MAGN1*( 2.0*RN3 - 1. 
MAGN1*( 2.0*RN4 - 1 
MAGN1*( 2. 1 
MAGN1*( 2. 1 


O*RN5 - 
O*RN6 - 


© Ooo 2 © 


c Adding the noise to measurement 


and encoder readings. 


T(1,4) 
T(2,4) 
T(3,4) 


TRETAL 
THETA2 
THETA3 
THETA4 
ΤΗΕΤΑΣ 
THETA6 


T(1,4) 
T(2,4) 
T(3,4) 


THETA] 
THETA2 
THETA3 
THETA4 
THETA5 


= THETA6 


+ RNX 
+ RNY 
+ RNZ 


+RN1 
+RN2 
+RN3 
+RN4 
+RN5 
+RN6 


c Storing the manipulator joint angles, calculated in the 
program JOINT and the theoretical measured tool pose, 
c calculated in this program, in the data file PUMA-POS.DAT. 


Ω 


WRITE (9,991) THETA1, THETA2, THETA3, THETA4, THETA5, THETA6 
WEITESISL992) L(17:1), T(1,2)79T(1,3),; 141,4) 

7) T(2,2),; T(2,;3), T(2,4) 

WRITE (9,992) T(3,1), T(3,2), T(3,3), T(3,4) 

WRITE (9,*) 


c | Format below decides the digits of accuracy of the 
c simulation data. 


991 
992 


FORMAT ( 6F12.6 ) 
FORMAT ( 3F16.10, F12.5 ) 


lIJoint vector data 
!Measurement data 


ENDDO 
WRITE (6,*) 'Data stored in F12.5, F12.4 format' 


CLOSE (8) 
CLOSE (9) 


STOP 


END 


c **SUBROUTINES** 


oes *SUBROUTINE 1** 
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1000 


SUBROUTINE RANDOM (X,Z) 


REAL FM, FX, Z 
INTEGER A, X, I, M 
DATA 1/17 


IF ( I.EQ. 0 ) GO TO 1000 
1-0 

M= 2 ** 20 

FM= M 

A= 2**10 + 3 


X= MOD( A*X ,M) 
FX= X 
Z= FX/ FM 


RETURN 


END 
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APPENDIX E PROGRAM ID6 


PROGRAM ID6 


c This is the program which identifies the kinematic parameters 
of the simulated PUMA 560 manipulator using the Non-linear 

c Least Squares method in IMSL routine 2XSSQ using function 

c PUMA ARM. The simulated poses are read from the data file 


ς 


ς 


Ω 


ΩΩ 


PUMA-POS.DAT 


& 


INTEGER LDFJAC, MM, M, NN, N, NSIG, MAXFN, IOPT, IXJAC, INFER, IER 
INTEGER I, J, K, NOBS, MAXNOBS 


REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


FJAC(LDFJAC,NN), XJTJ((NN41)*NN/2) 

PARM(4), F(LDFJAC), WORK((5*NN)+(2*MM)+((NN+1) *NN/2) ) 
DANGLE, DLENTH, TQ, DQ, EPS, DELTA, SSQ, SQERRI, SQERR2 
FIO, THO, SIO, PXO, PYO, PZO 

DT1, DT2, DT3, DT4, DT5 

DD1, DD2, DD3, DD4, DD5 

AA1, AA2, AA3, AA4, AAS 

ALI, AL2, AL3, AL4, ALS 

BL1, BL2, BL3, BL4, BL5 

DF6, TH6, SI6, PX6, PY6, PZ6, FI6 

TET1(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 
TET4(MAXNOBS), TET5(MAXNOBS), TET6(MAXNOBS) 
TM(3,4,MAXNOBS), SCALE, X(NN) 


PARAMETER (LDFJAC=6*360, MM=LDFJAC, NN=30) 
PARAMETER (MAXNOBS=360 ) 


EXTERNAL PUMA_ARM 


COMMON /PDATA/ NOBS, TM, SCALE, 


TETIS: TET2, TET3, TET4, TEIS, TET6 


Open data file for storage of output. This will be the identified 
kinematic parameters. 


OPEN (8, NAME-'RESULT.DAT', STATUS-'NEW') 


Open data file for the simulated poses. These were generated in 
program POSE. 


OPEN (9, NAME-'PUMA-POS.DAT', STATUS-'OLD') 


c Open data file of nominal kinematic parameters. 


OPEN (10,NAME-'INPUT.DAT', STATUS- 'OLD') 


Read nominal kinematic parameters from INPUT.DAT. 


read (10,*) 

read (10,*) £10,th0,810,px0,py0,pz0 
read (10,*) dtl,ddl,aal,all,bll 
read (10,*) dt2,dd2,aa2,a12,b12 
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read (10,*) dt3,dd3,aa3,a13,b13 

read (10,*) dt4,dd4,aa4,al4,bl4 

read (10,*) dt5,dd5,aa5,a15,b15 

read (10,*) 

read (10,*) df6,th6,s16,px6,py6,pz6 

read (10,*) 

read (10,*) nobs,n,dangle,dlenth,magnx,magni 


CLOSE (10) 
c Initialize the kinematic parameter data variables in the X array. 


X(1)=FI0 
X(2)=THO 
X(3)=SIO 
X(4)=Px0 
x(5)=PYO 
x(6)=PZ0 


X(7)2AA1 
X(8)=AL1 


X(9)=DT2 

X(10)=AA2 
X(11)=AL2 
X(12)=BL2 


X(13)=DT3 
X(14)=DD3 
X(15)=AA3 
X(16)=AL3 


X(17)=DT4 
X(18)=DD4 
X(19)=AA4 
X(20)=AL4 


X(21)=DT5 
X(22)=DD5 
X(23)=AA5 
X(24)=AL5 


X(25)=DF6 
X(26)-TH6 
X(27)=SI6 
X (28) =PX6 
X(29)=PY6 
X(30)=PZ6 


c Read simulated joint angles and tool pose from PUMA-POS.DAT. 


DO J = 1, NOBS 
READ (9,*) TET1(J), TET2(J), TET3(J), TET4(J), TET5(J), TET6(J) 
READ (9,*) TM(1,1,J), TM(1,2,J), TM(1,3,J), TM(1,4,J) 
READ (9,*) TM(2,1,J), TM(2,2,2J), TM(2,3,J), TM(2,4,J) 
READ (9,*) TM(3,1,J), TM(3,2,J7), TM(3,3,J), 1TM(3,4,J) 
READ (9,*) 
ENDDO 
CLOSE (9) 


C Set the scale factor for the direction cosines within the T6 matrix. 
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SCALE=100.0 


c The following lines set the parameters necessary for the operation 
c of the IMSL routine, ZXSSQ, for identification of the kinematic 
c parameters. 


c The subroutine which ZXSSQ is calling is PUMA ARM. 


NSIG-4 
EPS=0.0 
DELTA=0.0 
MAXFN=1500 
IOPT=1 
IXJAC=LDFJAC 
M=6*NOBS 


CALL ZXSSQ(PUMA ARM,M,N,NSIG,EPS,DELTA,MAXFN,IOPT, 
& PARM,X,SSQ,F,FJAC,IXJAC,XJTJ,WORK,INFER,IER) 


C Save identified kinematic parameters to data file RESULT.DAT. 


WRITE (8,*) 

WRITE (8,*) 'FIO, THO, SIO, PXO, PYO, PZO' 
WRITE (8,888) X(1), X(2), X(3), X(4), x(5), x(6) 
WRITE (8,*) 

WRITE (8,*) ‘'DT1, 501, Αλί, AL1, BLl' 

WRITE (8,888) 0.0, 0.0, X(7), X(8), 0.0 

WRITE (8,*) 

WRITE (8,*) 'DT2, DD2, AA2, AL2, BL2' 

WRITE (8,888) X(9), 0.0, X(10), X(11), X(12) 
WRITE (8,*) 

WRITE (8,*) 'DT3, DD3, AA3, AL3, BL3' 

WRITE (8,888) X(13), X(14), X(15), X(16), 0.0 
WRITE (8,*) 

WRITE (8,*) 'DT4, DD4, AA4, AL4, BL4' 

WRITE (8,888) X(17), X(18), X(19), X(20), 0.0 
WRITE (8,*) 

WRITE (8,*) ‘DTS, DD5, AA5, AL5, BL5' 

WRITE (8,888) X(21), X(22), X(23), X(24), 0.0 
WRITE (8,*) 

WRITE (8,*) 'DF6, TH6, SI6, PX6, PY6, PZ6' 
WRITE (8,889) X(25), X(26), X(27), X(28), X(29), X(30) 


888 FORMAT ( 5F12.5 ) 
889 FORMAT ( 6F12.5 ) 


c Calculate the root mean square error in identification of the 
¢ direction cosine and position parameters. 


TQ = DANGLE 
DQ = DLENTH 


c This calculates the error in identification of the direction 
c cosine parameters. 


SQERR1 = 
(FIO4TQ-X(1))**24(THO4TQ-X(2) ) **2*(SIO*TQ-X(3) ) **2 
*(ALl4TQ-X(8))**24(DT24TQ-X(9) ) **2* (AL24TQ-X(11) ) **2 
+(BL2+TQ-X (12) ) **2+(DT3+TQ-X (13) ) **2+(AL3+TQ-X(16) ) **2 
+(DT4+TQ-X(17) ) **2+(AL4+TQ-X (20) ) **2 


Mmm mM 
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& +(DTS+TQ-X(21) )**2+(ALS+TOQ-X(24) )**2 
& -*(DF6*TQ-X(25) ) **2*(TH64TQ-X(26) ) **24(SI64TQ-X(27) ) * *2 


SQERR1 - DSQRT( SQERR1/16 ) 


c This calculates the error in identification of the position 
C parameters. 


SQERR2 - 

(PX0+DQ-X(4) ) **2+( PY0+DQ-X(5) ) **2+(PZ0+DO-X(6))**2 
+(AA1+DQ-X(7))**2+(AA2+DQ-X(10) ) **2+(DD3+DQ-X(14) )**2 
+(AA3+DQ-X(15) )**2+(DD4+DQ-X(18) ) **2+(AA4+DQ-X(19))**2 
+ (DD5+DQ-X (22) ) **2+(AAS+DQ-X( 23) )**2 
+(PX6+DQ-X(28) ) **2+( PY6+DQ-X(29) ) **2+(PZ6+DQ-X(30))**2 
SQERR2 = DSQRT( SQERR2/14 ) 


Ων Ων Ων WM Mm 


c Write the position and orientation error to the data file 
c RESUBTSDAT. 


WRITE (8,*) 
WRITE (8,*) 'RMS PARMS (LENGTH), RMS PARMS (ANGLE)' 
WRITE (8,*) SQERR2, SQERR1 


c Write the position and orientation error to the screen. 


WRITE (6,*) 'RMS PARMS (LENGTH), RMS PARMS (ANGLE)' 
WRITE (6,*) SQERR2,SQERR1 


c Write the ZXSSQ convergence criteria to the data file 
C RESUDL.DAMS 


WRITE (8,*) 
WRITE (8,*) 'INFER, IER, NOBS, NSIG' 
WRITE (8,*) INFER, IER, NOBS, NSIG 
WRITE (8,*) 


c Write the ZXSSQ convergence criteria to the screen. 


WRITE (6,*) 'INFER, IER, NOBS, NSIG' 
WRITE (6,*) INFER, IER, NOBS, NSIG 


CLOSE (8) 
END 
c **SUBROUTINES*»* 
ο προσ ο I** 
SUBROUTINE PUMA_ARM (X, M, N, F) 
c This subroutine calculates the non-linear function for the use of 
c the IMSL routine ZXSSQ. It is the forward kinematic solution for 


c the PUMA manipulator. 


INTEGER M, N, II, JJ 
INTEGER I, J, K, NOBS, MAXNOBS 


REAL*8 X(N), F(M) 


REAL*8 FIO, THO, SIO; PXO το 
REAL*B DT1, DT2, DT3, Dia, vio 
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REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


DD1, DD2, DD3, DD4, DD5 

AAl, AA2, AA3, AA4, AAS 

πι, απο. AL3, ALA, ALS 

BL1, BL2, BL3, BL4, BL5 

FI6, TH6, SI6, PX6, PY6, PZ6, DF6 

TH1, TH2, TH3, TH4, THS 

TO(4,4), T1(4,4), T2(4,4), T3(4,4), T4(4,4) 
T5(4,4), T6(4,4), TRPY(4,4), TXYZ(4,4) 
TIMAT(4,4), T(4,4) 

TINV(4,4), TMJ(4,4), TDELTA(4,4) 
TET1(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 
TET4(MAXNOBS), TET5(MAXNOBS), TET6(MAXNOBS) 


REAL*8 TM(3,4,MAXNOBS), SCALE 


PARAMETER (MAXNOBS-360) 


COMMON /PDATA/ NOBS, TM, SCALE, 


& 


τετ. TET? τετ, τετ ΤΕΙ; τετὸ 


¢ Initializing the TIMAT matrix to an identity matrix. 


DATA TIMAT/ 1,0,0,0,0;,1,0,0,0,0,1,0,0,0,0,17 


c Assign kinematic parameters to the variable X array. 


FIO = 


THO 
510 
PXO 
PYO 
PZO 


DT1 
DD1 
AA1 
AL1 
BL1 


Ότο 
DD2 
AA2 
AL2 
BL2 


DT3 
DD3 
AA3 
AL3 


lt 


BL3 = 


DT4 
DD4 
BAG 
AL4 


it 


BL4 = 


DT5 
DD5 
AAS 
AL5 
BLS 


tt WW 


X(1) 
X(2) 
X(3) 
X(4) 
x (5) 
x (6) 


0.0 
0.0 
X(7) 
X(8) 
0.0 


X(9) 
0.0 
X(10) 
X(11) 
X(12) 


X(13) 
X(14) 
X(15) 
X(16) 
0.0 


X(17) 
X(18) 
X(19) 
X(20) 
0.0 


X(21) 
X(22) 


= X(23) 


X(24) 
0.0 
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DF6 = X(25) 


THE = X(26) 
SI6 = X(27) 
PX6 = X(28) 
PY6 = X(29) 
PZ6 = X(30) 


c Loop through NOBS times. 


K = 0 
DO J = 1, NOBS 


c Initializing the T matrix to an identity matrix. 


Dot gs 14 
DOI ENA 
T(11,JJ) = TIMATI JJ) 
ENDDO 

ENDDO 


c Calculate the actual manipulator joint angles. 


TH1 - DTl + TET1(J) 
TH2 = DT2 + TET2(J) 
TH3 = DT3 + TET3(J) 
TH4 = DT4 + TET4(J) 
THS = DTS « TET5(J) 
FI6 = DF6 + TET6(J) 


c Compute the T matrices, Tl thru T6. 


T3RPY( FIO, THO, SIO, TRPY ) 
T3XYZ( PXO, PYO, PZO, TXYZ ) 
MATMULC( TO, TRPY, TXYZ ) 


TRANSFORM ( ALl, AAl, DD1, THl, BL1, Tl 
TRANSFORM ( AL2, AA2, DD2, TH2, BL2, T2 
TRANSFORM ( AL3, AA3, DD3, TH3, BL3, T3 
TRANSFORM ( AL4, AA4, DD4, TH4, BL4, T4 
TRANSFORM ( AL5, AA5, DD5, TH5, BL5, T5 


hd E a -- 


T3RPY( FI6, TH6, SI6, TRPY ) 
T3XYZ( PX6, PY6, PZ6, TXYZ ) 
MATMULC(T6, TRPY, TXYZ ) 


ΕΕΕ ΕΕΕΕΕ ΕΕΕ 


ο Compute the complete transformation from the base frame to 
c the tool frame. 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Τι ) 
CALL MATMULA ( T, T2 ) 
CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, T5 ) 
CALL MATMULA ( T, T6 ) 


c Read the simulated measured T matrix for this observation: 


DO II = 1,3 
DO JJ = 1,4 
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TMIOII,JJ) 9 TM(II,JJ,J) 
ENDDO 
ENDDO 


c Remember that row 4 is 0, 0, 0, l. 


TMJ(4,1) = 0.0 
™J(4,2) = 0.0 
TMJ(4,3) * 0.0 
TMJ(4,4) » 1.0 


Compute the difference between the measured T matrix and the 
T matrix generated using the latest table of kinematic data. 


ΩΩ 


CALL MATSUB ( TDELTA, TM), T ) 
c Calculate the function F (six rows at a time). 


First, position. 


Ω 


K=K+1 
F(K) = TDELTA(1,4) 
K=K+1 
F(K) = TDELTA(2,4) 
K=K+1 
F(K) = TDELTA(3,4) 


c Now orientation. 


K=K+1 
F(K) = ((TDELTA(3,2)-TDELTA(2,3))/2.0) * SCALE 
K=K+#+1 
F(K) = ((TDELTA(1,3)-TDELTA(3,1))/2.0) * SCALE 
K=K+1 
F(K) = ((TDELTA(2,1)-TDELTA(1,2))/2.0) * SCALE 


c Ending the DO loop for J counter. 
ENDDO 

c Write RMS error in function F. 
XSSQ=0.0 
DO II=1,6*NOBS 

XSSQ=XSSO+F (II) *F(ITI) 

ENDDO 
XER=SORT( XSSQ) 


c Write RMS error to the screen. This allows the operator 
c to track how the identification process is proceeding. 


WRITE(6,*) XER 
RETURN 


END 
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PROGRAM VERIFY 


APPENDIX F PROGRAM VERIFY 


C This program generates the six-dof pose error for the PUMA manipulator. 

C It contains the identified calibration parameters and the exact parameter. 
C It uses a data file of verification joint angle sets POSEVER.DAT, and the 
C file RESULT.DAT from the program ID6. 


INTEGER I, J, K, NPOSES, N, nob 


REAL*8 


REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


COMMON 


DANGLE, DLENTH 


DT(5),dd(5),aa(5),al(5),b1(5) 
eDT(5),edd(5),eaa(5),eal(5),ebl(5) 

edf6, EFI6, ETH6, ESI6, EPX6, EPY6, ΕΡΖ6 

EDFO, EFIO, ETHO, ESIO, EPXO, EP60, EPZO 
THETA(1000,6), TDELTA(4,4) 

T0(4,4), T1(4,4), T20(4,4] T 

T4(4,4), T5(4,4), T6(4,4), TRPY(4,4), TXYZ(4,4) 
TIMAT(4,4), T(4,4), et(4,4) 


DFO, DTO, DSO, PXO, PYO, PZO 
οτι, DT2, DT3, DT4, DT5 
DD1, DD2, DD3, DD4, DD5 
ΔΑΙ, AA2, AA3, AA4, AAS 
AL1, AL2, AL3, AL4, AL5 
BL], BED2T"BDS;UBESJ BES 
DF6, DT6, DS6, PX6, PY6, PZ6 


TIMAT,THETA 


EXTERNAL FKS 


C Initialize the TIMAT matrix to an I matrix: 


DATA TIMAT/1,0,0,0,0,1,0,0,0,0, 1, 0,070,070) 


C Open data file 


OPEN (9, NAME-'posever.DAT',STATUS- OLD') 

OPEN (10, NAME-'input.DAT', STATUS-'OLD') 

OPEN (11, NAME-'result.DAT', STATUS- OLD') 

open (12, name-'error.dat',access-'append',status- 'unknown') 


c Read input 


read 
read 
read 
read 
read 
read 
read 
read 
read 
read 


parameters 


(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 


df0,dt0,;ds0O0, pxOo PL οσο εσο 
dtl,ddl,aal,all,bll 
dt2,dd2,aa2,al2,b12 
dt3,dd35 343,413, bl9 
dt4,dd4,aa4,al4,bl4 
dt5,dd5,aa5,a15,b15 


df6,dt6,ds6,px6,py6,pz6 
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read (10,*) nobs,n,dangle,dlenth,magnx,magnl 


CLOSE (10) 


C Read in joint angle sets for verification poses 


do i=1,nobs 

read(9,*)theta(i,1),theta(i,2),theta(i,3),theta(i,4), 
& theta(i,5),theta(i,6) 

enddo 

close (9) 


C Set exact link parameters for the manipulator: 


DT(1)20.0 
do 1=2,5 
dt(i)sdangle 

enddo 
DFO = DFO + Dangle 
DTO = DTO + DANGLE 
DSO = DSO + DANGLE 
PXO = PXO + DLENTH 
PYO = PYO + DLENTH 
PZO = PZO + DLENTH 
al(l) = all + DANGLE 
al(2) = al2 + DANGLE 
al(3) = al3 + DANGLE 
al(4) = al4 + DANGLE 
al(5) = al5 + DANGLE 
AA(1) = aal + DLENTH 
AA(2) = aa2 + DLENTH 
AA(3) = aa3 + DLENTH 
AA(4) = aad + DLENTH 
AA(5) = aaS + DLENTH 
DD(1) = ddl 1 defined 
DD(2) = dd2 1 defined 
DD(3) = dd3 + DLENTH 


DD(4) = dd4 + DLENTH 
DD(5) = dd5 + DLENTH 


BL(1) = bll defined 
BL(2) = bl2 + DANGLE 
BL(3) » bl3 
BL(4) = bl4 


BL(5) = bl5 


=e 


defined 
defined 
defined 


ome 


$— —_ 


DF6 = DF6 
DT6 = DT6 
DS6 = DS6 
PX6 = PX6 
PY6 = PY6 
PZ6 = PZ6 


Dangle 
DANGLE 
DANGLE 
DLENTH 
DLENTH 
DLENTH 


++eetet 


Cc Read in and set up estimated parameter table 
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READ (11,*) 


READ (11,*) 

READ (11,*) ΕΡΕΟ ΡΙΠΟ ΕΡΙΟ ΕΟΟ ΤΕ ο... 

ες 15:15 

read (11,*) 

read (11,*) 

read (11,*) edt(i),edd(i),eaa(i),eal(i),ebl(i) 
enddo 

read(11,*) 

read(11,*) 


read(11,*) edf6,eth6,esi6,epx6,epy6,epz6 


c Main loop through nobs joint angle sets 


do k=1,nobs 

call fks (k,DF0,DT0,DS0,PX0,PY0,PZ0,dt, al aa dda DI aTe: 
& DT6,DS6,PX6,PY6,PZ26,t) 

call fks (k,EDFO,ETHO,ESIO,EPXO,EPYO,EPZO,edt,eal,eaa,edd, 
& ebl,edf6,eth6,esi6,epx6,epy6,epz6,et) 


c Compute the differential tool matrix 


call matsub (tdelta,t,et) 


Ω 


Compute the pose errors 


poserr-sqrt(tdelta(1,4)**24tdelta(2,4)**24tdelta(3,4)**2) 
orerrl=(tdelta(3,2)-tdelta(2,3))/2 
orerr2=(tdelta(1,3)-tdelta(3,1))/2 
orerr3-(tdelta(2,1)-tdelta(1,2))/2 
orerr-sqrt(orerrl**24orerr2**24orerr3**2) 


c Update total error counts 


posterr-(poserr*(k-1l)*posterr)/k 
orterr =(orerr +(k-1)*orterr)/k 


c End of main lcop 


enddo 


write (6,*) ‘Position error, orientation error' 
write (6,*) posterr,orterr 


c write (6,*) 'how many nobs in this run?' 
c read (6,*) nob 
nob = 12 


write (12,*) 'for nobs of:' nob 

write (12,*) ‘position error, orientation error ' 
write (12,*) posterr,orterr 

write (12,*) 

close (12) 

end 


C ΑἉ ΑΛΑ ΜΑ Α ΑΛΑ ΛΑ ΑΛΑ dede ode dee ode dee eoe oen oe oed ἡ Δ ἡ ἡ ἡ ode ode dede dee dede ode dede ode ode dee dede oe ode eode on on n n ἡ x 


subroutine fks (n,DFO,DTO,DSO,PXO,PYO,PZO,dt,al,aa,dd,bl, 
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& GEG, UNO, 516,px6, pyo, p26, ©) 


REAL*8 TO(4,4), T1(4,4), Τ2(4,4), Τ3(4,4) 

REAL*8 T4(4,4), T5(4,4), T6(4,4), TRPY(4,4), TXYZ(4,4) 
REAL*8 TIMAT(4,4), T(4,4), dt(5),al(5),aa(5),dd(5),b1(5) 
REAL*8 DFO,DTO,DSO,PXO,PYO,PZO,DF6,TH6,SI6,PX6,PY6,PZ6,FI6 
real*8 theta(1000,6), ang(6) 

common timat,theta 


C Initialize the T matrix to an I matrix: 


DO Jwu1,4 
DO K=1,4 
T(J,K) = TIMAT(J,K) 
ENDDO 
ENDDO 


C Set up joint angles 


do i=1,5 
ang(i)=theta(n,i)+dt(i) 
enddo 


£i6=df6+theta(n,6) 
C Compute the T matrices, Tl thru T6: 


πι ου Ότο Ὀ ο τες 
D3SXrZO0pXO,DyU,pzO,TXY2ZE) 
MATMULC ( TO, TRPY, TXYZ ) 


TRANSFORM (al(1),aa(1),dd(1),ang(1),b1(1),T1) 
TRANSFORM (al(2),aa(2),dd(2),ang(2),b1(2),T2) 
TRANSFORM (al(3),aa(3),dd(3),ang(3),b1(3),T3) 
TRANSFORM (al(4),aa(4),dd(4),ang(4),b1(4) ,T4) 
TRANSFORM (a1(5),aa(5),dd(5),ang(5),b1(5),T5) 


T3RPY (£16,th6,si6,TRPY ) 
T3XYZ (PX6,py6,pz6,TXYZ ) 
MATMULC ( T6, TRPY, TXYZ ) 


ΕΕΕ ΕΕΕΕΕ ΕΕΕ 


C Compute the overall transformation, T: 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Tl ) 
CALL MATMULA ( T, T2 ) 
CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, T5 ) 
CALL MATMULA ( T, T6 ) 
return 

end 
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O000000000 


APPENDIX G PROGRAM LINSC 


PROGRAM linsc 


This program generates joint angles for the Puma manipulator 

arm. It presumes that the tool frame of the manipulator is 
constrained to move in the positive x direction only. The 

tool is constrained by a sliding linear scale. The values along 
the x direction are determined by a random number generator. The 
orientation of the tool is constrained as well. The x direction of 
tool is in the x direction of the WCF, the y direction of the tool 
is in the z direction of the WCF and the z direction of the tool is 
in the negative y direction of the WCF. 


INTEGER LDFJAC, M, N, obs, nobs 
PARAMETER (LDFJAC-12, M-LDFJAC, N56) 


real*8 fi0, thO, siO, pxO, pyO, pzO 
REAL*8 DT1, Ότο, Ότ. DTA Ὁ--- 

REAL*8 DD1, DD2, DD3., 153, bis 

REAL*8 AAl, AA2, AA3, AA4, AAS 

REAL*8 AL1, AL2, AL3, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 DF6, FI6, TH6, SI6, PX6, PY6, PZ6 


REAL*8 RN1,RN2,RN3,RN4,RN5,RN6 
REAL*8 RN7,RN8,RNS,RN10,RN11,RN12 
REAL*8 RN13,RN14,RN15,RN16,RN17,RN18 


INTEGER infer,ier,iopt,nsig,maxfn 


REAL*8 FJAC(LDFJAC,N), xjtj((n*1)*n/2), xjac(1ldfjac,n) 
REAL*8 parm(4), f(ldfjac), work((5*n)*(2*m)*((n*1)*n/2)) 
REAL*8 X(N) 

REAL*8 magnx,magnl 


EXTERNAL PUMA ARM 


INTEGER I, J, K, nou 
REAL*8 TDES(4,4), T(4,4), SCALE, DANGLE, DLENTH, NUM 


COMMON /PDATA/ TDES, DANGLE, DLENTH, T 

COMMON /KIN/ fiO, thO, siO, pxO, pyO, pzo, 
DT1,DT2,DT3;DT4 DTI 5. 
AL1,AL2,AL3,AL4,AL5, 
AA1],AA2,AA3,AA4,AA5, 
DD1,DDZ;,DD3 ,DD4a bas, 
BL1,BL2,BL3,BL4,BL5, 
DF6,TH6,SI6,PX6, PY6,PZ6 


PM MM Mm m 


C Initialize data variables 
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obs=0 
C Open data files for input 


OPEN (10, NAME-'puma-pos.dat', STATUS-'NEW') 
OPEN (9, NAME-'input.dat', STATUS-'OLD') 


C Read input kinematic data 


read (9,*) 

read (9,*) £10,th0,8i0,px0,py0,pz0 
read (9,*) dt1,ddl,aal,all,bll 
read (9,*) dadt2,dd2,aa2,al2,bl2 
read (9,*) dat3,dd3,aa3,al13,b13 
read (9,*) dt4,dd4,aa4,al4,b14 
read (9,*) dt5,dd5,aa5,al15,b15 
read (9,*) 

read (9,*) df6,th6,si6,px6,py6,pz6 
read (9,*) 

read (9,*) nobs,nou,dangle,dlenth,magnx,magn1l 


close (9) 
C Adjust nominal values 


£10=f10+dangle 
tho=th0+dangle 
$10=si0+dangle 
px0=px0+dlenth 
py0=py0+dlenth 
pz0=pz0+dlenth 


dt120.0 

dt2=dt2+dangle 
dt3=dt3+dangle 
dt4=dt4+dangle 
dt5=dt5+dangle 


all=alli+dangle 
al2=al2+dangle 
al3=al3+dangle 
al4=al4+dangle 
al5=al5+dangle 


aal=aal+dlenth 
aa2=aa2+dlenth 
aa3=aa3+dlenth 
aa4=aa4+dlenth 
aa5=aa5+dlenth 


ddis0.0 
dd2=0.0 
dd3=dd3+dlenth 
dd4=dd4+dlenth 
dd5=dd5+dlenth 


bll-bll 
b12=bl2+dangle 
b13=b13 
bl4-bl4 


b15=b15 


df6=df6+dangle 
th6=th6+dangle 
si6=si6+dangle 
px6=px6+dlenth 
py6=py6+dlenth 
pz6=pz6+dlenth 


Ω ΩΩ ΩΩ 


C Get random number seed 


write (6,*) ‘Type in a 6-digit random number seed' 
read (5,*) iseed 


C Start of main loop 
1010 obs=obs+1 
C Set joint angles to zero 


do i=l,n 
x(i)=0.0 
enddo 


C Get random bar angles 


1000 call random (iseed,num) 
num-num*900.0 


C Establish desired tool pose 


do 11=1,4 

do 17-51, 4 
TDES(11,33)20.0 

enddo 

enddo 


TDES(1,4)= num 
TDES(4,4)= 
TIDES (1, 1)= 
TDES(3,2)= 
TDES (2,3) = <1; 


C Call IMSL ZXSSQ for inverse kinematic solution 
nsig=4 
eps=0.0 
delta=0.0 
maxfn=500 
iopt-1l 
ixjac-1ldfjac 


CALL 2XSSQ(puma arm,m,n,nsig,eps,delta,maxfn,iopt,parm,x, 
& ssq,f,xjac,ixjac,xjtj,work,infer,ier) 


C Check for singularities 
if (ssq .gt. 0.00001) goto 1000 


C Print results to 2 decimal places 
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write(6,*) obs,ssq 


C Generate the random noise 


CALL RANDOM (ISEED,RN1) 

CALL RANDOM (ISEED,RN2) 

CALL RANDOM (ISEED,RN3) 

CALL RANDOM (ISEED,RN4) 

CALL RANDOM (ISEED,RN5) 

CALL RANDOM (ISEED,RN6) 

CALL RANDOM (ISEED,RN7) 

CALL RANDOM (ISEED,RNS) 

CALL RANDOM (ISEED,RN9) 

CALL RANDOM (ISEED,RN10) 

CALL RANDOM (ISEED,RN11) 

CALL RANDOM (ISEED,RN12) 

CALL RANDOM (ISEED,RN13) 

CALL RANDOM (ISEED,RN14) 

CALL RANDOM (ISEED,RN15) 

CALL RANDOM (ISEED,RN16) 

CALL RANDOM (ISEED,RN17) 

CALL RANDOM (ISEED,RN18) 

RN1 = MAGNX * (2.0 * RN1 - 1.0) 
RN2 = MAGNX * (2.0 * RN2 - 1.0) 
RN3 = MAGNX * (2.0 * RN3 - 1.0) 
RN4 = MAGNI * (2.0 * RN4 - 1.0) 
RN5 = MAGN] * (2.0 * RNS - 1.0) 
RN6 = ΜΑΟΝΙ “ (2.0 * RN6 - 1.0) 
RN? MAGN1 * (2.0 * RN? - 1.0) 
RN8 = MAGN1 * (2.0 * RN8 - 1.0) 
RN9 MAGN1 * (2.0 * RN9 - 1.0) 
RN10 = MAGN] * (2.0 * RN10 - 1.0) 
RN11 = MAGN] * (2.0 * RN11 - 1.0) 
RN12 - MAGN1 * (2.0 * RN12 - 1.0) 
RN13 = MAGN1 * (2.0 * RN13 - 1.0) 
RN14 = MAGN1 * (2.0 * RN14 - 1.0) 
RN15 = MAGN] * (2.0 * RN15 - 1.0) 
RN16 = MAGN1 * (2.0 * ΕΝΙ6 - 1.0) 
RN17 - MAGN1 * (2.0 * RN17 - 1.0) 
RN18 » MAGN1 * (2.0 * RN18 - 1.0) 
T(1,4) » T(1,4) * RN1 

T(2,4) = 1(2,4) + RN2 

T(3,4) = 1(3,4) + RN3 

T(1,1) = T(1,1) + RN4 

T(1,2) = T(1,2) + RNS 

T(1,3) = T(1,3) + RN6 

T(2,1) = T(2,1) + RN? 

T(2,2) = T(2,2) * RN8 

T(2,3) = T(2,3) * RN9 

T(3,1) = T(3,1) * RN10 

T(3,2) » T(3,2) + RN11 

T(3,3) » T(3,3) * RN12 


X(1) = X(1) * RN13 
X(2) = X(2) + RN14 


991 
992 


X(3) 
X(4) 
X(5) 
X(6) 


write 
write 
write 
write 
write 
write 


X(3) 
X(4) 
X(5) 
X(6) 


(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 


* RN15 
* RN16 
t RN17 
* RN18 


X(1),X(2),X(3),X(4),X(5),X(6) 
T(1,1),T(1,2),T(1,3),T(1,4) 


T(2,1),T(2,2),T(2,3),T(2,4) 
T(3,1),T(3,2),T(3,3),T(3,4) 


FORMAT ( 6F12.6 ) 
FORMAT ( 3F16.10, F12.5 ) 


C Continue for other bar angles 


11 {655 .1lt. nobs) go co,1010 


CLOSE 


END 


(10) 


G κ κ κ de de e ἃ ἡ he fede oe de efe efe de ehe fee obe he eee oe fede oe fefe ode fe ode ode ode ode ode eode σε πε defe ehe ode e cfe ok fee eoe oe one n o x n x Xx 


SUBROUTINE PUMA ARM (X,M,N,F) 


C This subroutine calculates the non-linear function for the use of 
C the IMSL routine ZXSSQ. It is the forward kinematic solution for 
C the PUMA manipulator. 


DMM movit Mm 


INTEGER M, N 
REAL*8 X(N), F(M) 


INTEGER II, 


real*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


REAL*8 
REAL*8 
REAL*8 
REAL*8 


τας, 
Dm 
DD1, 
AAl, 
ALI, 
BL1, 
DF6, 


THl, 
TO(4, 
T5(4, 


JJ 
tho, 
Ότο, 
DD2, 
AAZ, 
AL2, 
BL2, 
FI6, 


$10, 
Dis; 
DD3, 
AA3, 
AL3, 
BL3, 
ΤΗ, 


px0, 
DT4, 
DD4, 
AAG, 
AL4, 
BL4, 
SiG; 


ΡΥΟ, Ρ20 

DT5 

DD5 

AAS 

AL5 

BL5 

PX6, PY6, Ρ26 


TH2, TH3, TH4, THS 
4), T1(4,4), T2(4,4), T3(4,4), T4(4,4) 
4), T6(4,4), trpy(4,4), txyz(4,4) 


TIMAT(4,4), T(4,4), td(4,4) 


INTEGER I, J, K 
REAL*8 TDES(4,4), DANGLE, DLENTH,scale 


COMMON /PDATA/ TDES, DANGLE, DLENTH, T 
COMMON /KIN/ f£i0,th0,si0,px0,py0,pz0, 


DT1,DT2,DT3,DT4,DT5, 
AL1,AL2,AL3,AL4,AL5, 
AA1,AA2,AA3,AA4,AA5, 
DD1,DD2,DD3,DD4,DD5, 
BL1,BL2,BL3,BL4,BL5, 
DF6,TH6,SI6,PX6,PY6,PZ6 


C Initialize the TIMAT matrix to an I matrix: 
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DATA TIMAT, 1,0,70,0,0,1,0,0,0,0,1,0,0,0,0,1/ 


scal 


e=100.0 


C Initialize the T matrix to an I matrix 


DO II * 1,4 
DO JJ = 1,4 

T(I 
ENDDO 
ENDDO 

C Manipulator joint angles 

TH1 = DTl + X(1) 
TH2 - DT2 + X(2) 
TH3 - DT3 + X(3) 
TH4 = DT4 + X(4) 
THS = DTS + X(5) 
FI6 = DF6 + X(6) 


C Compute the 


call 
call 
call 


t AMT 


C Compute the overall transformation, T: 


». ΕΕΕΕΕΕΕ 


πι πατε ο) 


T matrices, Tl thru T6: 


t3rpy (f10,th0,si0, trpy) 


t3xyz (px0,py0,pz0, txyz) 
matmulc (tO,trpy,txyz) 


TRANSFORM ( 
TRANSFORM ( 
TRANSFORM ( 
TRANSFORM ( 
TRANSFORM ( 


πα, 
AL2, 
AL3, 
AL4, 
ALS, 


λλ1, 551, 
AA2, DD2, 
AA3, DD3, 
AA4, DD4, 
AAS, DDS, 


TH1, 
TE2, 
TH3; 
TH4, 
τα. 


t3rpy ( fi6, th6, SiG, trpy } 
T3XYZ ( PX6, PY6, PZ6, txyz ) 
matmulc ( t6, trpy, txyz) 


MATMULA ( T, TO ) 
MATMULA ( T, T1) 
MATMULA ( T, T2 ) 
MATMULA ( T, T3 ) 
MATMULA ( T, T4 ) 
MATMULA ( T, T5 ) 
MATMULA ( T, T6 ) 
matsub(td,tdes,t) 


C Calculate the function F 


£(1) 
. 
fco 


=td(1,4) 
-td(2,4) 
-td(3,4) 


f(4)-td(1,1)*scale 
£(5)=td(1,2)*scale 
£(6)=td(1,3)*scale 
£(7)=td(2,1)*scale 
£(8)=td(2,2)*scale 
£(9)=td(2,3)*scale 
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BEIG 
BL2, 
ΕΙ, 
BL4, 
BL5, 


τι 
T2 
Tq 
T4 
TS 


C — c —— X s 


£(10)=td(3,1)*scale 
£(11)=td(3,2)*scale 
£(12)=td(3,3)*scale 


sum = 0.0 
do i=1,12 
xssq-sumtf (i) 
enddo 

c write (6,*) xssq 


RETURN 
END 


C ktit EEE EKER EEAEEEEKREAKEAKKRKEKE KEKE 


SUBROUTINE RANDOM (χ,Ζ) 


C This subroutine generates random numbers in the range 0-1 
C using a supplied seed x, the returned random number being z. 


REAL FM, FX, Z 
INTEGER A, X, I, M 
DATA I/1/ 


IF I .EOQ. 0 J οοτο οσο 
1Ξ0 

ος 

FM= M 

ιτ. 


1000 X» MOD( A*X ,M) 
FX- X 
Z- FX/ FM 


RETURN 
END 
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APPENDIX H PROGRAM ACDAT 


program acdat 


This program is the data acquisition program for the linear 

slide version of the puma manipulator calibration experiments. 

It requires that the value for the distance down the linear 

slide and the six joint angles of the puma when it is in that 
configuration. The pose data is saved in the file "slide-pos.dat." 
That file is set up in the append mode. 


ΩΩΩΩΩΩ 


real*8 x, thl, th2, th3, th4, th5, th6 
real*8 tm(4,4) 


open (10, name-'slide-pos.dat',access-'append',status-'unknown!') 
C Inputting the distance down the linear slide. 


100 write (6,*) 'input the value for x' 
read (6,*) x 


C Inputting the six joint angles. 


write (6,*) 'input the 6 joint angles' 
read 6,*) thi; th2, th3, th4, th5, th6 


C This will set the T6 matrix for the value of x measured. 


do 11=1,4 
do 3j=1,4 
tm(ii,j3)-20.0 
enddo 
enddo 


tm(1,4)= x 
tm(4,4)= T: 
tm(1l1,1)= 1.0 
tm(3,2)= 210 
tm(2,3)= -1.0 


C Writing the pose data to "slide-pos.dat" 


write (10,991) thl, th2, th3, th4, th5, th6 
write (10,*) 

write (10,992) tm(1,1),tm(1,2),tm(1,3),tm(1,4) 
write (10,992) tm(2,1),tm(2,2),tm(2,3),tm(2,4) 
write (10,992) tm(3,1),tm(3,2),tm(3,3),tm(3,4) 
write (10,992) tm(4,1),tm(4,2),tm(4,3),tm(4,4) 
write (10,*) 


write (6,*) 


write (6,*) ‘data saved' 
write (6,*) 
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write (6,*) ‘do you wish to make another observation’ 
write (6,*) ‘enter ail for yes or a 0 for no' 

write (6,*) 

read (6,*) ans 


if {5π5 -eq. 1) ao το 108 


991 FORMAT ( 6F12.6 ) 

992 FORMAT ( 3F16.10, F12.5 } 
close(10) 
stop 
end 
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APPENDIX I PROGRAM ID6 LINSC 


PROGRAM ID6 LINSC 


C Robot Identification using the Non-linear Least Squares method. 
C Simulation data is read for the PUMA manipulator from 
C the data file PUMA-POS.DAT 


C Change parameter LDFJAC to change the number of observations, 
C set LDFJAC = 6 * Number of observations 


& 


INTEGER LDFJAC, MM, M, NN, N, NSIG, MAXFN, IOPT, 
PARAMETER (LDFJAC=12*42, MM=LDFJAC, NN=26) 


IXJAC, INFER, IER 


REAL*8 FJAC(LDFJAC,NN), XJTJ( (NN4+1)*NN/2) 

REAL*8 PARM(4), F(LDFJAC), WORK((5*NN)+(2*MM)+((NN+1)*NN/2) ) 
REAL*8 X(NN) 

EXTERNAL PUMA ARM 


REAL*8 
REAL*8 


DANGLE, DLENTH, TQ, 
SQERR1, SQERR2 


DO, EPS, DELTA, SSQ 


f10 th0.s10,DX0,pyO,p2z0 
ΠΤΙ Ότο Dia, DT4, DT5 
ΕΙ ο, Όρο, Ὁμά, DD5 
AAI, AA2, AA3, AA4, AAS 
AL1, AL2, AL3, AL4, ALS 
BL1, BLZ, BL3, BL4, BLS 
DF6, TH6, SI6, PX6, PY6, P26, FI6 


real*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


INTEGER I, J, K, NOBS, MAXNOBS 
PARAMETER (MAXNOBS=360) 
REAL*8 TET1(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 
REAL*8 TET4(MAXNOBS), TET5(MAXNOBS), TET6(MAXNOBS) 
REAL*8 TM(3,4,MAXNOBS), SCALE 
COMMON /PDATA/ NOBS, TM, SCALE, 

τετ TET2. TET3, TET4, TET5, TET6 


C Open data files for inputs and results 


c Read input 


OPEN 
OPEN 
OPEN 


(8, NAME-'RESULT.DAT', STATUS-'NEW') 
(9, NAME-'PUMA-POS.DAT', STATUS- 'OLD') 
(10,NAME='INPUT.DAT', STATUS-'OLD') 


parameters 


read 
read 
read 
read 
read 
read 
read 
read 
read 


(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 
(10,*) 


fj10 -thOrgi0,pxO,pyO,pzO 
dtl,ddl,aal,all,bll 
dt2,dd2,aa2,a12,b12 
dt3,dd3,a2a3,al3,b13 
dt4,dd4,aa4,al4,b14 
dt5,dd5,aa5,a15,b15 


df6,th6,si6,px6,py6,pz6 


111 


read (10,*) 
read (10,*) nobs,n,dangle,dlenth,magnx,magnl 


CLOSE (10) 


write (6,*) 'enter nobs' 
read (6,*) nobs 


C Initialize data variables 


X(1)=£10 
X(2)=tho 
X(3)=s10 
X(4) =px0 
x(5)=py0 
x(6)=pz0 


X(7)2AA1 
X(8)=AL1 


X(9)=DT2 

X(10)=AA2 
X(11)=AL2 
X(12)=BL2 


X(13)=DT3 
X(14)=DD3 
X(15)2AA3 
X(16)=AL3 


X(17)=DT4 
X(18)=DD4 
X(19)=AA4 
X(20)=AL4 


X(21)=DT5 
X(22)=DD5 
X(23)=AA5 
X(24)=AL5 


x(25)2df6 
x(26)=th6 
x(27)=si6 
x(28)=px6 
x(29)=py6 
x(30)=pz6 


0000 


C Read simulated joint data and tool pose 


DO J = 1, NOBS 
READ (9,*) TET1(J), TET2(J), TET3(J), TETS(0), TEts 3) coe 
read (9,*) 
READ (9,*) TM(1,1,J), μ.ο τ. Ὁ ΙΤ, 7; 
READ (9,*) TM(2,1,J), TM(2,2,J), πα TM(2,4,J) 
READ (9,45) T™M(3,1,J), TM(3,2,J), ποσα, TM(3,4,J) 
READ (9,*) 

c READ (9,*) 
ENDDO 
CLOSE (9) 


C Initialize scale for the angular rows of the Jacobian 
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SCALE=100.0 


C Call IMSL routine for non-linear identification 


& 


NSIG=3 


EPS=0.0 
DELTA-0.0 
MAXFN-1500 


IOPT-1 


IXJAC-LDFJAC 
M=12*NOBS 


CALL ZXSSQ( PUMA_ARM,M,N,NSIG, EPS, DELTA, MAXFN, IOPT, 


PARM, X,SSQ,F,FJAC, IXJAC, XJTJ,WORK, INFER, IER) 


C Save results to data file 


888 
889 


WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
WRITE 


(8,*) 

(8;*) fiO; πο 5:0, px0, Ῥγ0, pzO' 
(8,888) X(1), X(2), X(3), X(4), x(5), x(6) 
(8,*) 

(8,*) 'DT1, DD1, AA1, AL1, BL1' 

(e 888) 0.0, 0.0, X(7), X(8), 0-0 

ο) 

(8,*) DIZ, DD2, AAZ, AL2, BL2' 

(87888) Χίο ο ο Χίο, Χο X(12) 
(8,3) 

(B,*) 'DT3, DD3, AA3, AL3, BL3' 

(8,888) X(13), X(14), X(15), X(16), 0.0 
(85 *) 

(8,*) 'DT4, DD4, AA4, AL4, BL4' 

(8,888) X(17), X(18), X(19), X(20), 0.0 
(8,*) 

(Ses) DT5, DDS, AA5, AL5, BL5' 

(87888) X(21), X(22), X(23), X(24), 0.0 
(8,*) 

(8,*) 'DF6, TH6, SI6, PX6, PY6, PZ6' 


(8,889) x(25),x(26), si6, px6, py6, pz6 


FORMAT ( 5F12.5 ) 
FORMAT ( 6F12.5 ) 


C Calculate root mean square error in identification 


TQ = DANGLE 
DQ = DLENTH 


C Error in identification (angular parameters) 


Ων Ων Ων Ων Ω͂ν Mm 


SQERRI 


(FIO+TQ-X(1))**2 +(THO+TQ-X(2))**2 +(SIO+TOQ-X(3) )**2 
+(DT3+TQ-X(13))**2 +(DT4+TQ-X(17) )**2 +(DT5+TQ-X(21))**2 
+(AL1+TQ-X(8))**2 +(AL2+TQ-X(11) )**2 

+(AL3+TQ-X(16))**2 +(AL4+TQ-X(20))**2 +(AL5+TQ-X(24)) **2 
+(BL2+TQ-X(12))**2 +(DT2+TQ-X(9) )**2 

+(af64+tg-x(25))**2 +(th6t+tq-x(26) )**2 

SQERR1 = DSQRT( SQERR1/15 ) 


C Error in identification (length parameters) 
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em Qe m Mm 


SQERR2 = 

(PXO*DQ-X(4))**2 *(AA140.04DQ-X(7))**2 *(AA24DQ-X(10))**2 
*(AA34DQ-X(15))**2 *(AA4*DQ-X(19))**2 *(AA54DQ-X(23) ) **2 
+(PY0+DQ-X(5))**2 +(PZ0+DQ-X(6) )**2 

+ (DD3+DQ-X(14))**2 +(DD4+DQ-X(18))**2 +(DD5+DQ-X(22))**2 
SQERR2 = DSQRT( SQERR2/11 ) 


WRITE (8,*) 

WRITE (8,*) ‘RMS PARMS (LENGTH), RMS PARMS (ANGLE) ' 
WRITE (8,*) SQERR2, SQERRI 

WRITE (6,*) 'RMS PARMS (LENGTH), RMS PARMS (ANGLE)' 
WRITE (6,*) SQERR2,SQERRI 

WRITE (8,*) 

WRITE (8,*) 'INFER, IER,NOBS,NSIG' 

WRITE (8,*) INFER, IER,NOBS,NSIG 

WRITE (6,*) 'INFER, IER,NOBS,NSIG' 

WRITE (6,*) INFER, IER,NOBS,NSIG 

WRITE (8,*) 

CLOSE (8) 

END 


C πλ ΝΑ ΑΛΑ ΝΑ ΑΛΑ ΧΑΑ ΧΑ ΑΛΑ ΑΛΑ ΑΛΑ ΑΛΑ ΑΛΑ ἡ κ κ ἡ κ κ ἃ ἡ ἡ ἡ κ ἡ κ ἡ ἡ ἡ ἡ ἡ κ κ αὶ κ ἡ κα ἡ κ κ ἡ ἡ ἡ κα ἡ ἡ κ ἡ ἀ 


SUBROUTINE PUMA_ARM (X, M, N, F) 


C This subroutine calculates the non-linear function for the use of 
C the IMSL routine DUNLSF. It is the forward kinematic solution for 
C the PUMA manipulator. 


INTEGER M, N 
REAL*8 X(N), F(M) 


INTEGER II, JJ 


real*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


£10, th0;s20; px0, py0,pz0 
DTl, DI2; Ότο τι DEUS 
DD1, DD2, DD3, DD4, DD5 
AAl1, AA2, AA3, AA4, AA5 
AL1, AL2, AL3, AL4, AL5 
BL1, BL2, BL3, BL4, BL5 
FI6, TH6, SI6, PX6, PY6, PZ26, DF6 


REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


TH1, TH2, TH3, TH4, TH5 

TO(4,4), T1(4,4), T2(4,4), T3(4,4), T4(4,4) 
T5(4,4), T6(4,4), TRPY(4,4), TXY2(4,4) 
TIMAT(4,4), T(4,4) 

TINV(4,4), TMJ(4,4), TDELTA(4,4) 


INTEGER I, J, K, NOBS, MAXNOBS 
PARAMETER (MAXNOBS=360) 
REAL*8 TET1(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 
REAL*8 TET4(MAXNOBS), TET5(MAXNOBS), TET6(MAXNOBS) 
REAL*8 TM(3,4,MAXNOBS), SCALE 
COMMON /PDATA/ NOBS, TM, SCALE, 

& TETl, TET2, TET3, TET4, TETS, TET 
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C Initialize the TIMAT matrix to an I matrix: 
DATA ΠΗ 4070 ,0,0,0,4,0,0,0,0,1,0;,0,0,07 17 


C Set parameters for the manipulator: 


£i0 = X(1) 
tho = X(2) 
810 = X(3) 
pxO = X(4) 
pyO = x(5) 
pz0 = x(6) 
DT1 = 0.0 
DD1 = 0.0 
AAl = X(7) 
AL1 = X(8) 
BL1 = 0.0 
DT2 = X(9) 
DD2 = 0.0 
Aa? = X(10) 
AL2 = X(11) 
BL2 = X(12) 
DT3 = X(13) 
DD3 = X(14) 
AA3 = X(15) 
AL3 = X(16) 
BL3 = 0.0 
DT4 = X(17) 
DD4 = X(18) 
AAS = X(19) 
AL4 = X(20) 
BL4 = 0.0 
DTS = X(21) 
DDS = X(22) 
AAS = X(23) 
AL5 = X(24) 
BL5 = 0.0 
DF6 = x(25) 
TH6 = x(26) 
SI6 = 0.0 !x(27) 
px6 = 0.0 !x(28) 
py6 = 0.0 1x(29) 


pz6 = 55.9 !x(30) 
C Loop NOBS times 


K = 0 
DO J = 1, NOBS 


C Initialize the T matrix to an I matrix 


DO II = 1,3 
DO JJ = 1,4 

T(II,JJ) = TIMAT(II,JJ) 
ENDDO 


[15 


ENDDO 


C Manipulator joint 
TH1 = DT1 + 
TH2 = DT2 + 
TH3 = DT3 + 
TH4 = DT4 + 
THS = DT5 + 
FI6 = DF6 + 


angles 


TET1(J) 
TET2 (J) 
TET3(J) 
TET4 (J) 
TETS (J) 
TET6(J) 


C Compute the T matrices, Tl thru T6: 


call t3rpy(fi0,th0,si0, trpy) 
call t3xyz(px0,py0,pz0, txyz) 
call matmulc(tO,trpy,txyz) 


TRANSFORM 
TRANSFORM 
TRANSFORM 
TRANSFORM 
TRANSFORM 


ΕΕΕ ΕΕΕΕΕ 


C Compute the overall transformation, T: 


MATMULA 
MATMULA 
MATMULA 
MATMULA 
MATMULA 
MATMULA 
MATMULA 


411111: 


C Get the "T-measured" 


μπι οσο) τοπ στα. ο) 


DO II = 1,4 
DO JJ = 1,4 
ENDDO 
ENDDO 
c tmj(3,2)=-1.0 
ο tmj(2,3)=1.0 
™J(4,1) - 0.0 
TMJ (4,2) = 0.0 
TMI (4,3) = 0.0 
TMJ (4,4) = 1.0 


( ALI, 
( AL2, 
( AL3, 
( AL4, 
( ALS, 


TO 
TE 
a 
T3 
T4 
T5 
T6 


= = 


`~ 


- 


`~ 


Be r1 r3 


`~ 


AAl, 
AA2, 
AA3, 
AAG, 
AAS, 


DD1, 
DD2, 
DD3, 
DD4, 
DD5, 


THI, 
της, 
TH3, 
TH4, 
του, 


T3RPY ( FI6, TH6, 516, TRPY ) 
T3XYZ ( PX6, PY6, PZ6, TXYZ ) 
MATMULC (T6, TRPY, TXYZ ) 


BL1, 
BL2, 
BL3, 
BL4, 
BL5, 


T1 
T2 
T3 
T4 
T5 


matrix for this observation: 


C Compute the elements of the "delta-Tn"': 


CALL MATSUB ( TDELTA, TMJ, T ) 


C Calculate the function F (six rows at a time) 


f (k+1)=tdelta(1,4) 
f(k*2)-tdelta(2,4) 
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we eee eee See” 


£(k+3)=tdelta(3,4) 


£(k+4)=tdelta(1,1)*scale 
£(k+5)=tdelta(1,2)*scale 
f£(k+6 )=tdelta(1,3)*scale 
£(k+7)=tdelta(2,1)*scale 
f(k*8)ztdelta(2,2)*scale 
f(k*9)2tdelta(2,3)*scale 
f£(k+10)=tdelta(3,1)*scale 
f£(k+11)=tdelta(3,2)*scale 
f£(k+12)=tdelta(3,3)*scale 


k=k+12 

C End the do-loop for counter J 
ENDDO 

C Write RMS error in F 
XSSQ-0.0 
DO II=1,12*NOBS 

XSSQ-XSSQ4F(II)*F(II) 

ENDDO 


XER-SQRT ( XSSQ) 
WRITE(6,*) XER 


RETURN 
END 
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